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FOREWORD 


B 1950 年 图 灵 在 他 的 著作 《计算 机 器 和 短 能 》 中 抛 出 “机 器 是 否 能 思考 ”的 话题 起 ， 人 类 
开始 致力 于 机 器 人 的 研究 与 应 用 。 从 1954 年 诞生 的 世界 第 一 个 工业 机 器 人 Unimate 用 单个 机 械 
臂 运输 压铸 件 并 焊接 ， 开 始 改变 制造 业 ， 到 现在 的 人 形 机 器 人 能 像 人 一 样 行动 、 感 知 、 计 算 、 处 
理 、 表 达 ， 机 器 人 已 逐渐 走 进 工业 、 农 业 、 安 防 、 医 疗 、 教 育 、 家 庭 .……… 成 为 推动 新 时 代 变 
革 的 重要 科技 力量 。 

时 代 变 革 呼 唤 人 的 变革 , 呼唤 具备 机 器 人 及 人 工 智 能 理论 知识 与 应 用 技巧 的 新 时 代 人 才 。 当 
TA. 在 呼唤 的 同时 ， 也 赋予 了 大 家 更 好 的 成 才 条 件 。 四 十 年 前 ,我 们 很 难 找到 中 国 自 主 研发 的 领 
先 的 机 器 人 学 习 研 究 载体 , 这 在 一 定 程 度 上 加 大 了 研究 难度 ， 限 制 了 核心 技术 的 攻关 速度 。 但 四 
十 年 后 ， 像 Roban 专业 级 双 足 人 形 机 器 人 这 样 能 开源 拓展 的 AI 展示 平台 及 ROS 应 用 平台 ， 将 
为 人 工 智能 、 机 械 电 子 工程 、 自 动 控制 、 计 算 机 等 专业 的 学 生 们 提供 更 好 的 学 习 条 件 ， 让 他 们 
更 好 地 理解 机 电 原理 、 人 机 交互 、SLAM、 视 觉 算 法 、Python、ROS 等 理论 知识 ， 并 且 掌 握 实 操 
技能 。 

我 们 看 到 了 机 器 人 所 带 来 的 生产 与 生活 变革 ， 也 看 到 了 变革 中 的 不 足 。 这 些 不 足 是 核心 技 
术 还 不 足以 满足 产品 创新 的 需求 ; 应 用 场景 还 不 足以 满足 人 们 生活 的 期 待 ， 专业 人 才 还 不 足以 
支撑 产业 的 快速 发 展 。 然 而 ， 这 些 不 足 对 于 我 们 来 说 是 机 遇 ， 是 读者 未 来 可 以 进驻 的 领域 。 我 
想 用 雪 莱 的 话 来 与 各 位 机 器 人 爱好 者 、 学 习 者 、 研 究 者 共勉 :“ 人 不 能 创造 时 机 ， 但 是 可 以 抓 住 
那些 已 经 出 现 的 时 机 。” 希 望 看 到 这 篇 序言 的 读者 ， 不 仅 看 到 了 时 机 ， 也 能 努力 抓 住 时 机 ， 成 为 
市 场 需求 的 高 技术 人 才 ! 


孙 立 宁 
苏州 大 学 机 电学 院 院 发 ， 长 江 学 者 特聘 教授 
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PREFACE 


自从 发 明 机 器 人 以 来 ， 对 两 足 机 器 人 【具有 两 条 腿 的 机 器 人 ) 的 研究 一 直 持续 地 进行 ， 特 
别 是 仿 人 型 双 足 机 器 人 。 创 造 类 似 于 人 的 机 器 人 并 使 其 不 进行 任何 改变 就 可 以 在 相同 的 人 类 工 
作 环 境 中 使 用 ， 意 味 着 它们 被 建造 用 来 模仿 人 类 和 人 类 的 行为 。 这 类 机 器 人 与 轮 式 机 器 人 相 比 ， 
能 更 加 有 效 地 适应 复杂 环境 。 双 足 机 器 人 在 人 机 交互 和 复杂 地 形 的 适应 方面 有 天 然 优 势 。 如 果 
运动 平衡 问题 解决 得 好 ， 再 用 人 工 智 能 技术 当 大 脑 ， 仿 人 机 器 人 将 具备 广阔 的 应 用 场景 和 巨大 
的 商业 化 价值 一 一 不 仅 可 在 机 场 、 酒 店 、 养 老 等 服务 行业 广泛 应 用 ， 而 且 在 高 校 教具 、 娱 乐 影 
视 、 军 用 装备 等 方面 也 具有 重要 价值 。 随 着 人 工 智 能 的 发 展 ， 在 未 来 的 生产 、 生 活 中 ， 仿 人 型 
双 足 机 器 人 可 以 帮助 人 类 解决 很 多 问题 ， 比 如 完成 送 餐 、 驮 物 、 抢 险 、 采 矿 等 一 系列 危险 或 繁 
重 的 工作 。Roban 机 器 人 是 一 款 高 端 仿 人 机 器 人 ,拥有 讨 人 喜欢 的 外 形 ， 具 有 人 工 智能 ， 能 够 在 
视听 方面 与 人 类 互动 。Roban 机 器 人 的 相关 技术 完全 是 开源 的 , 开放 给 所 有 的 高 等 教育 项 目 , 支 
持 在 Roban 上 的 开发 及 教育 相关 领域 的 研究 。 

Roban 机 器 人 支持 Linux、Windows 或 Mac OS 等 环境 下 的 编程 开发 ， 开 放 的 编程 构架 支持 
C++ 或 Python 语言 。 无 论 使 用 者 专业 水 平 如 何 ， 都 可 以 通过 编程 平台 与 Roban 机 器 人 进行 编程 。 

本 书 介绍 Roban 机 器 人 相关 基础 知识 及 编程 操作 ， 全 书 共 分 8 章 ， 内 容 如 下 。 

第 1 章 Roban 机 器 人 概述 。 介绍 Roban 机 器 人 系统 、 关 节 运 动 模型 、 机 器 人 操作 系统 框架 、 
Roban 机 器 人 基本 操作 、 网 络 连 接 设置 和 远程 登录 。 

第 2 章 Python 编程 基础 。 介 绍 Python 的 基本 语法 、 函 数 、 对 和 象 与 类 、 文 件 和 异常 。 

第 3 Æ ROS 使 用 概述 。 介 绍 ROS 的 程序 包 与 节点 、 话 题 与 服务 、 文 件 与 参数 及 调试 工具 ， 
最 后 给 出 了 ROS 安装 和 配置 、 主 从 机 设置 实例 及 消息 通信 实例 。 

第 4 章 同步 定位 与 地 图 构建 。 介 绍 SLAM 技术 中 对 图 像 的 接收 和 发 布 、 位 置 的 定位 和 图 像 
追踪 、 八 又 树 存储 方式 及 平面 图 的 生成 、 路 径 生 成 及 控制 机 器 人 行走 。 

第 5 章 V-REP 使 用 概述 。 介 绍 V-REP 仿真 环境 的 搭建 ， 提 供 了 Roban 机 器 人 在 V-REP 环 
境 下 的 导入 和 配置 ， 以 及 仿真 环境 下 的 传感器 配置 ， 最 后 给 出 机 器 人 大 赛 仿 真 环境 的 应 用 示例 。 

第 6 章 Roban 机 器 人 运动 控制 基础 。 介 绍 Roban 机 器 人 相关 的 基础 结构 、 动作 控制 方法 , 以 
及 机 器 人 姿态 的 运动 学 正 、 道 求解 方法 ， 最 后 给 出 控制 Roban 机 器 人 避 障 行走 实例 。 
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第 7 章 双 足 步 行 基础 。 介 绍 Roban 机 器 人 相关 的 运动 学 基础 、ZMP 含义 以 及 基于 线性 倒立 
摆 生 成 双 是 行走 步 态 ， 最 终 给 出 Roban 机 器 人 行走 及 上 下 楼 梯 实 例 。 

第 8 章 人 机 交互 。 介 绍 Roban 机 器 人 音频 及 视频 处 理 的 硬件 基础 、 相 关 理论 及 相关 应 用 , 最 
后 给 出 人 脸 识别 及 数字 识别 的 人 工 智 能 实践 。 

本 书 各 章 内 容 相对 独立 ， 在 内 容 安排 上 按照 先 易 后 难 的 原则 编写 。 前 面 章节 解释 的 语句 后 
面 再 次 出 现时 不 做 解释 ， 读 者 在 学 习 时 尽量 按照 章节 顺序 阅读 和 调试 程序 。 书 中 罗列 相应 内 容 ， 
如 人 参数、 软件 安装 、 程 序 调试 方法 等 ， 供 需要 时 查阅 。 

本 书面 向 初学 者 ， 对 机 器 人 学 相关 理论 、Python 语言 、 视 觉 及 声学 知识 等 介绍 得 相对 简单 ， 
仅 选 择 调试 机 器 人 所 必须 掌握 的 基础 知识 ， 书 中 所 使 用 的 范例 也 不 涉及 复杂 算法 。 读 者 在 掌握 
Roban 机 器 人 系统 的 基础 知识 、 开 发 设计 思路 后 ， 可 以 参考 开源 WIKI， 查 阅 所 提供 的 更 多 API. 

本 书 可 作为 Roban 机 器 人 的 操作 手册 和 编程 参考 书 ， 也 可 作为 高 等 学 校 计 算 机 及 相关 专业 
“机 器 人 程序 设计 ”课程 的 教材 。 

由 于 作者 水 平和 经 验 有 限 ， 书 中 琉 漏 之 处 在 所 难免 ， 敬 请 读者 指正 。 
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Roban 机 器 人 概述 


CHAPTER 1 


Roban 机 器 人 是 一 款 基于 ROS〈 机 器 人 操作 系统 ) 的 人 工 智 能 人 形 机 器 人 。 本 章 首 先 介绍 
Roban 机 器 人 的 系统 组 成 和 运动 模型 ， 然 后 介绍 一 些 Roban 机 器 人 的 基本 操作 与 开发 方法 。 


11 Roban 机 器 人 简介 


Roban 机 器 人 的 控制 系统 本 质 上 就 是 一 台 安 装 有 Linux 系统 的 计算 机 。 该 计算 机 上 安装 了 
机 器 人 专用 的 软件 系统 ， 配 合 机 器 人 本 体 上 的 其 他 软 硬 件 系统 即 可 达到 机 器 人 的 控制 目标 。 


11.1 Roban 机 器 人 系统 


Roban 机 器 人 身高 68cm， 体 重 6.5kg， 主 要 硬件 包括 CPU、 主 板 、 扬 声 器 、 麦 克 风 阵列 、 深 
度 相 机 、ToF 测 距 传感器 、 电 机 、 语 音 合成 器 、 陀 螺 仪 等 。 图 1.1 所 示 为 Roban 机 器 人 。 

1. 通用 硬件 系统 

(1) 处 理 器 (CPU): 主 处 理 器 采用 8 f Intel i3-8109U 处 理 器 ， 主 频 3.0~3.6GHz、4MB 高 
速 缓存 、 双 核 四 线程 。 基 于 Cortex M4 处 理 器 作为 协 处 理 器 ， 用 于 传感器 数据 收集 以 及 运动 数 
据 转 发 。 

(2) 存储 器 : 内 存 8GB， 固 态 硬盘 120GB. 

(3) 网 络 连接 : 以 太 网 IRJAS 接口 、Intel i219-V 10/100/1000M/s。 支 持 无 线 网 络 连 接 、 
Wireless-AC 9560. IEEE 802.11ac 2x2。 蓝 牙 支 持 V5 版 本 。 

(4) 外 部 接口 : 两 个 USB3.0 端口 、 一 个 标准 HDMI2.0A 接口 、 一 个 雷电 3 接口 。 

(5) 电源 锂电 池 : 动力 锂电 池 最 高 电压 为 12.6V., 电池 容量 4000mA-h, 2A 电流 充电 约 需 2h。 

(6) 视觉 与 声音 系统 : 机 器 人 视觉 的 硬件 基础 是 相机 摄像头)，Roban 机 器 人 搭载 了 两 个 
摄像 头 ， 可 以 用 于 拍摄 图 像 ， 录 制 视频 以 及 VSLAM 导航 ， 可 以 通过 调用 相关 接口 使 机 器 人 具 
有 认 知 功能 。 
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相机 : Roban 机 器 人 提供 了 两 个 相机 和 一 个 位 于 头 部 的 Realsense D435 RGBD 深度 摄像 头 ， 
除了 可 以 得 到 通常 的 RGB 图 像 之 外 ， 还 可 获取 到 分 辨 率 为 1280x720 像素 的 深度 信息 ， 可 以 提 
供 最 高 30 帧 / 秒 的 RGB 图 像 以 及 90 帧 / 秒 的 深度 图 像 ， 这 是 机 器 人 进行 V-SLAM 导航 的 基础 。 

声音 系统 : Roban 机 器 人 可 以 “ 听 到 ”声音 ， 并 且 可 以 辨别 出 声音 方向 ， 还 可 以 “说 ”出 悦 
耳 的 声音 ， 听 和 说 的 硬件 是 传声器 (俗称 麦克 风 ) 和 扬声器 。 机 器 人 后 背 安 装 了 2 个 2W 的 扬 
声 器 用 于 机 器 人 音频 的 输出 。 机 器 人 头 部 安装 有 6 个 麦克 风 阵 列 ， 通 过 6 个 麦克 风 可 以 计算 音 
源 的 方位 角 ， 对 于 唤醒 方向 的 声音 实现 定向 收音 ， 从 而 可 以 实现 其 与 人 的 互动 。 

2. 软件 系统 

Roban 机 器 人 操作 系统 为 Linux 的 一 个 十 分 常见 的 发 行 版 Ubuntu16.04LTS， 在 这 个 操作 系 
统 的 基础 上 构建 了 基于 ROS 的 基础 包 框 架 ， 其 支持 Linux、Window 或 Mac OS 等 操作 系统 的 远 
程控 制 ， 既 可 以 直接 通过 ssh 对 该 系统 上 的 程序 进行 修改 ， 也 可 以 通过 ROS 的 消息 机 制 对 机 器 
人 进行 控制 。 由 于 机 器 人 本 身 就 搭载 了 一 个 计算 机 ， 开 发 者 也 可 以 使 用 外 置 的 鼠标 键盘 以 及 显 
示 器 直接 连 机 器 人 进行 编程 ， 还 可 以 直观 地 观察 机 器 人 运行 时 的 各 种 数据 。 

为 了 更 加 方便 地 对 机 器 人 的 硬件 进行 操作 ，Roban 机 器 人 在 ROS 的 基础 上 还 构建 了 多 层 结 
构 用 于 对 机 器 人 进行 操作 ， 这 些 包 都 采用 ROS 的 消息 机 制 以 及 Service 机 制 进行 了 连接 ， 从 而 
可 以 方便 地 使 用 各 种 ROS 支持 的 语言 对 机 器 人 进行 良好 的 操控 。Roban 的 软件 架构 如 图 1.2 所 
示 ， 分 为 底层 〈 驱 动 层 入 中 间 层 以 及 应 用 层 ， 开 发 的 过 程 主要 是 通过 对 应 用 层 进行 修改 和 开发 ， 
从 而 使 得 机 器 人 可 以 按 设计 逻辑 运行 。 
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动作 调试 手势 识别 应 用 层 
动作 包 Beesley PIE 


图 1.2 Roban 的 软件 架构 


3. 机 器 人 特有 硬件 

CD 深度 摄像 头 。Roban 机 器 人 的 头 部 安装 有 一 个 D435 深度 摄像 头 , 除了 可 以 提供 RGB 的 
图 像 数 据 之 外 还 可 以 提供 深度 数据 。 摄 像 头 会 投射 出 红外 结构 光 ， 摄 像 头 有 两 个 红外 相机 ， 可 
以 获取 到 红外 数据 从 而 得 到 深度 信息 , 而 在 室外 的 环境 中 ， 由 于 结构 光 投 射 距离 有 限 , 深度 摄 
像 头 会 直接 采用 外 部 的 纹理 信息 ， 利 用 双 目 摄像 头 的 原理 对 深度 进行 计算 。 有 了 深度 摄像 头 之 
后 ， 可 以 使 得 机 器 人 更 好 地 获取 前 方 的 障碍 物 信息 ， 也 可 以 用 于 V-SLAM 导航 相关 的 应 用 ， 最 
近 测 量 距离 约 0.1m， 最 远 可 测量 10m。 深 度 相 机 原理 结构 如 图 1.3 所 示 。 


右 侧 相机 红外 结构 光 投影 左 侧 相机 RGB 相机 


1.3 D435 结构 图 


(2) ToF 测 距 传感器 。Roban 机 器 人 的 胸 前 额外 安装 有 一 个 基于 飞行 时 间 原 理 的 测 距 传 感 
器 ， 是 为 了 精确 测量 与 障碍 物 之 间 的 距离 ,可 以 测量 2m 内 的 准确 距离 , 采用 的 是 垂直 腔 面 发 射 
激光 器 基础 。 通 过 发 射 940nm 的 红外 激光 ， 并 且 通 过 测量 从 发 射 激光 到 收 到 反射 激光 的 时 间 来 
判断 检测 距离 内 是 否 有 障碍 物 ， 如 果 一 段 时 间 内 没有 收 到 反射 的 激光 ， 则 认为 有 效 距 离 内 没有 
障碍 物 。 

(3) 惯性 传感器 。 惯 性 传感器 用 于 测量 Roban 机 器 人 的 身体 状态 及 加 速度 ， 包 括 陀螺 仪 和 
加 速度 计 ， 通 过 这 两 个 传感器 的 数据 融合 可 以 实现 对 机 器 人 姿态 的 估计 。 

(4) 关节 位 置 编码 器 。 关 节 位 置 编码 器 用 于 测量 机 器 人 自身 关节 的 位 置 ， 且 在 各 个 关节 内 
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可 用 于 各 关节 位 置 的 反馈 ， 使 用 这 些 位 置 传感器 ， 机 器 人 在 步行 的 过 程 中 可 以 更 好 地 估计 机 器 
人 本 体 的 位 姿 。 

(5) 压力 感应 器 。 机 器 人 每 只 脚 上 有 4 个 压力 传感器 〈Force Sensitive Resistors, FSR), 用 于 
确定 每 只 脚 压 力 中 心 (重心 ) 的 位 置 。 在 行走 过 程 中 ，Roban 机 器 人 会 根据 重心 位 置 进行 步 态 调 
整 以 保持 身体 平衡 ， 同 时 也 可 以 用 于 判断 机 器 人 的 脚 是 否 着 地 ， 为 步 态 算法 的 研究 提供 了 方便 。 

(6) 发 光 三 极 管 。Roban 机 器 人 的 前 胸 有 一 排 发 光 二 极 管 ， 可 编程 使 得 其 显示 不 同 的 状态 ， 
可 用 于 机 器 人 状态 显示 。 

(7) 可 编程 按键 。Roban 机 器 人 的 后 背 具 有 轻 触 按键 ,可 编程 将 其 作为 状态 输入 ， 用 于 机 器 
人 状态 的 切换 。 

(8) 机 器 人 关节 。 控 制 机 器 人 的 关节 可 以 使 机 器 人 完成 各 种 动作 ，Roban 机 器 人 有 22 个 独 
立 的 直流 伺服 关节 ， 根 据 具体 位 置 不 同 使 用 了 三 种 不 同 的 电机 及 减速 比 ， 电 机 的 转动 通过 齿轮 
的 减速 之 后 可 驱动 机 器 人 的 关节 ， 完 成 各 种 关节 运动 ， 从 而 使 机 器 人 具有 强大 的 运动 能 力 。 


1.1.2 Roban 机 器 人 关节 运动 模型 


1. Roban 机 器 人 坐标 系 

机 器 人 做 各 种 动作 时 需要 驱动 机 器 人 各 关节 的 电机 动作 。 为 描述 机 器 人 各 种 动作 的 实现 过 
程 ， 使 用 如 图 1.4 所 示 的 笛 卡 儿 坐 标 系 。 其 中 x 轴 指 向 机 器 人 身体 前 方 ,，y 轴 为 机 器 人 由 右 向 左 
方向 ，z 轴 为 垂直 向 上 方向 。 


E 1.4 Roban 机 器 人 坐标 系 示意 
2. 关节 运动 分 类 
对 于 连接 机 器 人 两 个 身体 部 件 的 关节 来 说 ， 驱 动 电 机 实现 关节 运动 时 ， 固 定 在 躯 于 上 的 部 
件 是 固定 的 ， 远 离 身 干 的 部 件 将 围绕 关节 轴 旋 转 。 沿 z 轴 方向 的 旋转 称 为 偏转 〈yaw)， 沿 y f 
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方向 的 旋转 称 为 俯仰 (pitch)， 沿 x 轴 方 向 的 旋转 称 为 横 深 roll)。 沿 关节 轴 逆 时 针 转 动 角 度 为 
正 ， 顺 时 针 转 动 角 度 为 负 。 

3. 关节 命名 规则 

关节 按照 先 脚 后 手 的 ID 顺序 进行 命名 , 为 了 实现 一 些 动作 可 能 需要 不 同 关节 相互 配合 才 可 
以 实现 ， 其 中 Roban 机 器 人 的 各 关节 的 了 数值 如 图 1.5 所 示 。 


L5 Roban 机 器 人 关节 ID 分 布 
4. 关节 运动 范围 
机 器 人 的 每 个 关节 都 有 一 定 的 运动 范围 ， 例 如 图 1.6 就 表示 机 器 人 头 部 俯仰 方向 上 的 运动 
范围 如 下 :其 低头 方向 上 运动 范围 为 33"， 抬 头 方向 上 的 运动 范围 为 24°。 


1.6 机 器 人 头 部 俯仰 方向 上 的 运动 范围 


在 运动 模型 中 ， 规 定 逆 时 针 转 动 为 正 ， 顺 时 针 方 向 为 负 ， 图 1.6 中 所 示意 的 21 号 关节 的 运 
动 范围 是 [一 0.418，0.6108]。 特 别 需 要 注意 的 是 ， 头 部 的 两 个 关节 运行 范围 会 出 现 看 合 现象 ， 即 
在 头 部 左右 转动 时 俯仰 方向 的 转动 会 受到 影响 ， 各 关节 具体 运动 范围 可 查阅 本 书后 续 章节 或 机 
器 人 参考 手册 。 
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5. Roban 机 器 人 的 自由 度 

机 器 人 可 以 独立 运动 的 关节 称 为 机 器 人 的 运动 自由 度 ， 简 称 为 自由 度 (Degree of Freedom, 
DOF) 。Roban 机 器 人 的 头 部 有 两 个 关节 ， 可 以 进行 偏转 和 俯仰 运动 ， 因 此 头 部 的 自由 度 为 2。 
Roban 机 器 人 除了 具有 运动 自由 度 之 外 ， 每 只 手 还 可 以 张 开 或 闭合 ， 各 具有 一 个 自由 度 ， 因 此 
Roban 机 器 人 共 具 有 22 个 自由 度 。 


11.3 Roban 机 器 人 控制 框架 


基于 ROS，Roban 机 器 人 构建 了 底层 和 中 间 层 的 用 于 操作 机 器 人 的 API， 通 过 这 些 API 可 
以 方便 地 对 机 器 人 运动 、 语 音 、 视 频 等 方面 进行 操作 ， 满 足 机 器 人 的 使 用 需求 。 在 应 用 层 的 开 
发 中 可 以 使 用 任意 一 种 ROS 所 支持 的 语言 对 应 用 层 程 序 进 行 开 发 ， 都 可 以 达到 正确 的 控制 机 器 
人 行为 的 目的 。 通 过 对 于 这 些 相关 API 的 调用 ， 可 以 在 不 了 解 执行 器 具体 原理 的 情况 下 方便 开 
发 者 开发 出 机 器 人 的 应 用 程序 。 

尽管 Roban 的 各 个 不 同 模块 相互 之 间 差 异 很 大 ， 但 在 使 用 的 过 程 中 使 用 ROS 的 MSG 和 
Service 机 制 ， 采 用 标准 的 ROS 消息 机 制 来 表示 信息 ， 而 且 各 个 模块 的 权限 管理 机 制 也 是 相似 
的 ， 这 种 方式 使 得 在 调用 不 同 的 API 时 具有 相似 的 编程 模式 ， 降 低 了 Roban 机 器 人 程序 设计 的 
复杂 性 。 

在 机 器 人 上 的 开发 可 以 使 用 C++. Python 或 者 其 他 ROS 支持 的 编程 语言 ， 但 是 不 管 使 用 哪 
种 编程 语言 ， 实 际 的 编程 方法 都 是 相似 的 。 为 了 便于 使 用 者 调试 ， 建 议 用 户 在 开发 应 用 的 过 程 
中 使 用 Python 语言 进行 行为 层 的 控制 ， 而 对 时 间 和 效率 敏感 的 控制 代码 用 C++ 实现 ， 以 提高 运 
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本 节 将 介绍 一 些 Roban 机 器 人 的 基本 操作 ， 以 及 Roban 机 器 人 开发 相关 的 基础 知识 。 


1.2.1 无 线 网 络 设置 


Roban 机 器 人 可 以 通过 有 线 网 络 或 WiFi 的 方式 连接 计算 机 。 由 于 有 线 网 络 需 要 接 网 线 ， 因 
此 推荐 Roban 机 器 人 使 用 无 线 WiFi 的 方式 进行 连接 ，Roban 机 器 人 完成 网 络 配置 后 可 以 记忆 无 
线 网 络 密码 并 且 再 次 开机 时 可 自动 连接 上 次 连接 过 的 无 线路 由 器 ， 配 置 Roban 机 器 人 无 线 网 络 
的 步骤 如 下 : 

(1) 给 Roban 机 器 人 接 上 外 置 电源 ， 接 上 外 置 显示 屏 与 鼠标 、 键 盘 。 

(2) 将 机 器 人 按 图 1.7 所 示 的 方式 放置 ， 打 开 电 源 开关 ， 等 待 约 1min， 机 器 人 启动 完成 后 ， 
机 器 人 会 从 蹲 下 状态 变 为 站 立 状 态 ， 此 时 机 器 人 即 启动 完成 。 
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等 待 约 lmin 


图 1.7 开机 


(3) 显示 屏 显示 机 器 人 上 Ubuntu 系统 的 图 形 界面 ， 如 图 1.8 Pras. 
è 


1.8 WiFi 设置 1 


(4) 从 右上 角 的 WiFi 列表 选择 需要 连接 的 WiFi， 如 图 1.9 所 示 。 


19 WiFi 设置 2 
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(5) 在 弹出 的 密码 框 中 输入 你 所 需要 连接 WiFi 的 密码 ， 如 图 1.10 所 示 ， 并 且 单 击 Connect 
按钮 。 


6 
" 
D 
5 
5 
B 
2 


图 1.10 WiFi ix & 3 


(6) 通过 Cte T 快捷 键 打 开 一 个 终端 , 然后 输入 ifconfig 并 回 车 , 即 可 得 到 当前 机 器 人 的 下 
地 址 ， 如 图 1.11 所 示 ， 通 过 该 P 地 址 可 对 机 器 人 进行 远程 访问 。 


IPDIÓGET I TTETI 
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1.2.2 ”远程 登录 Roban 机 器 人 


虽然 Roban 机 器 人 可 以 通过 外 接 键 盘 、 鼠 标 、 显 示 器 实现 对 程序 的 修改 功能 ， 但 是 在 执行 
程序 的 过 程 中 可 能 也 会 让 机 器 人 运动 ， 很 多 程序 会 让 机 器 人 执行 不 同 程度 的 运动 ， 因 此 推荐 通 
过 ssh 连接 的 方式 来 对 Roban 机 器 人 进行 开发 。 有 很 多 ssh 的 客户 端 可 供 选用 ， 本 书 推荐 一 种 功 
能 齐全 且 免 费 使 用 的 远程 工具 MobaXterm. 
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MobaXterm 是 远程 处 理 的 终极 工具 箱 。 在 一 个 单独 的 Windows 应 用 程序 中 ， 为 程序 员 、 网 
站 管理 员 、IT 管理 员 和 几乎 所 有 需要 以 更 简单 的 方式 处 理 远程 工作 的 用 户 提供 了 大 量 的 功能 。 
MobaXterm 为 用 户 提供 了 多 标签 和 多 终端 分 屏 选项 ， 内 置 SFTP 服务 以 及 Xerver， 让 用 户 可 以 
远程 运行 X 窗口 程序 ，SSH 连接 后 会 自动 将 远程 目录 展示 在 SSH 面板 中 , 方便 用 户 上 传 下 载 文 
件 。MobaXterm 提供 了 所 有 重要 的 远程 网 络 工 具 、 协议 SSH, X11, RDP, VNC, FTP, MOSH 
等 ) 和 UNIX 命令 (bash、ls、cat、sed、grep、awk、rsync 等 ) 到 Windows 桌面 。RDP 类 型 的 
会 话 可 以 直接 连接 Windows 远程 桌面 ， 比 Windows 自 带 的 mstsc 要 方便 不 少 。 

MobaXterm 的 官方 网 站 为 https://mobaxterm.mobatek.net/ ， 可 以 方便 地 从 其 官网 下 载 到 客户 


端 ， 程 序 运行 后 界面 如 图 1.12 所 示 。 
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1.12 MobaXterm 界面 


在 对 应 界面 中 选中 Session 选项 ， 如 图 1.13 所 示 。 
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1.13 MobaXterm 远程 连接 界面 


aur cá 


在 对 应 的 Session 选项 卡 中 选择 登录 方式 ， 如 图 1.14 所 示 。 


一 
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在 选取 的 Session 选项 卡 中 选择 SSH 的 登录 方式 ， 如 图 1.15 所 示 ， 选 取 指 定 用 户 名 ， 默 认 
KAP 4A lemon, 实际 的 用 户 名 以 及 密码 可 以 在 登录 之 后 修改 , 如 果 已 更 改过 即 按照 更 改 后 的 
用 户 名 填写 即 可 。 


| Remote host [FEZ 760304 — | — ASpecity wremame [emm J Por 2 图 


1.15 MobaXterm 的 SSH 登录 


在 进入 之 后 会 要 求 输入 密码 ， 如 图 1.16 所 示 。 


z 


1.16 MobaXterm 的 密码 输入 
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在 输入 密码 后 即 可 进入 如 图 1.17 所 示 的 界面 ， 该 示意 图 的 左 侧 为 文件 管理 界面 ， 可 以 方便 


地 使 用 拖 电 的 方式 管理 Roban 机 器 人 上 的 文件 和 本 机 的 文件 ， 
钮 对 文件 进行 操作 。 在 窗口 右 侧 是 一 个 终端 界面 ， 可 以 直接 使 


le 
la 
|æ 


wilt 
JOUT 1 


ne ore tem oem 人 入 


$211 
H 
E 


fit 


iN 
h IH 


fi 
iH 


HT 


1.17 MobaXterm 远程 界面 


1.2.3 ”使 用 VS Code 开发 


也 可 以 使 用 界面 上 部 的 那 一 排 按 
有 命令 行 对 机 器 上 的 终端 操作 。 


前 面 介绍 了 使 用 MobaXterm 远程 登录 Roban 机 器 人 进行 开发 的 方法 ， 通 过 MobaXterm 将 
本 地 编辑 好 的 程序 文件 上 传 到 Roban 机 器 人 中 运行 ， 但 代码 不 能 在 机 器 人 上 进行 调试 ， 也 不 方 
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便 使 用 调试 软件 ， 为 了 调试 方便 可 采用 Visual Studio Code (简称 VS Code) 进行 开发 。 下 面 介 
绍 如 何 使 用 VS Code 对 机 器 人 进行 开发 。 

首先 ， 需 要 在 VS Code 的 官方 网 站 下 载 和 系统 匹配 的 VS Code 版 本 ，VS Code 的 官网 地 址 
为 https;//code.visualstudio.com/. 

然后 ， 到 VS Code 扩展 页 面 安装 如 图 1.18 所 示 的 Remote Development 扩展 插件 ， 这 个 插件 
会 自动 安装 一 系列 远程 开发 所 需要 的 捅 件 ， 安 装 完成 后 即 可 用 于 Roban 机 器 人 的 远程 连接 开发 。 


图 1.18 Remote 扩展 安装 


安装 完成 后 ， 按 Ctrl + Shift + P 组合 键 打开 VS Code 功能 键 界面 ， 如 图 1.19 所 示 ， 在 其 中 
选取 Connect Current Window to Host 的 选项 ， 然 后 就 可 以 开始 连接 远程 机 器 人 进行 开发 了 。 


图 1.19 Remote 连接 


在 第 一 次 尝试 远程 连接 机 器 人 时 ， 如 图 1.20 所 示 ， 单 击 对 应 的 新 建 SSH 主机 设置 。 


图 1.20 新 增 主机 配置 
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然后 配置 对 应 的 远程 主机 IP 地 址 和 用 户 名 ， 如 图 1.21 所 示 ， 其 中 下 地 址 为 Roban 机 器 人 
的 实际 下 地 址 ， 用 户 名 如 果 没 有 变更 过 ， 使 用 默认 的 lemon 即 可 。 


121 主机 名 称 设 置 


然后 输入 对 应 的 用 户 密码 ， 如 图 1.22 所 示 ， 如 果 没 有 变更 过 ， 输 入 密码 softdev。 


图 1.22 主机 密码 设置 


在 输入 密码 后 ， 选 择 对 应 的 远程 主机 系统 ， SUR] 1.235. Roban MEARAN Ubuntu 
系统 ， 在 选择 远程 主机 时 选择 Linux, Audi ARIAS ER Roban d RUN, YOR es ae - 些 VS 
Code 的 Host 端的 插件 ， BRA AS BERE: 装 插件 。 


1.23 主机 系统 设置 
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在 安装 完成 后 就 可 以 使 用 VS Code 对 机 器 人 进行 远程 开发 了 ， 如 图 1.24 所 示 ， 左 边 部 分 为 
文件 区 ， 可 以 对 文件 进行 操作 ; 右 侧 有 对 应 的 编码 区 及 终端 区 ， 可 用 于 机 器 人 软件 的 开发 。 


1.24 VS Code 远程 界面 


Python 编程 基础 


CHAPTER 2 


Python 是 一 种 解释 型 、 面 向 对 象 、 动 态 数据 类 型 的 高 级 程序 设计 语言 。Python 是 跨 平 台 的 
开发 工具 ， 可 以 在 多 个 操作 系 上 进行 程序 设计 ， 包 括 Windows, Linux 和 MacOS X. Python 提 
供 了 非常 完善 的 基础 代码 库 , 覆盖 了 网 络 、 文 件 、GUI,、 数据 库 、 文本 等 内 容 。 除 了 内 置 的 库 外 ， 
Python 还 有 大 量 的 第 三 方 库 ， 可 以 通过 网 络 免费 下 载 安装 使 用 。 

目前 ， 通 用 的 Python 有 两 个 版 本 ， 分 别 是 Python 2.x 和 Python 3.x。 这 两 个 版 本 并 不 兼容 ， 
部 分 语句 的 语法 有 差别 。 不 过 Python 官方 提供 了 可 将 Python 2.x 代码 转换 成 为 Python 3.x 的 代 
码 工具 ， 以 便 程序 设计 者 使 用 。 

Roban 机 器 人 使 用 Python 3.5。 本 章 主要 介绍 Windows 平台 下 Python 3.x 的 基本 应 用 。 


2.1 Python 语法 
2.1.1 Python 运行 方式 


Python 的 解释 器 python.exe 位 于 Python 的 安装 目录 ， 运 行 Python 源 程序 需要 使 用 解释 器 进 
行 解释 。 目 前 ， 常 用 的 运行 方式 有 三 种 ， 分 别 是 通过 命令 管理 器 运行 Python 脚本 ， 通 过 Python 
自 带 的 IDLE 运行 Python 脚本 ， 以 及 通过 集成 开发 环境 运行 Python 脚本 。 

1. 命令 管理 器 

同时 按 下 键盘 上 的 Windows ##45 R 键 ， 并 在 输入 框 中 输入 “cmd”， 按 Enter 键 即 可 进入 命 
令 管理 器 。 在 命令 管理 器 中 输入 “Python” 并 按 Enter 键 即 可 运行 Python 脚本 。 

在 命令 管理 器 中 运行 Python 脚本 的 方式 有 以 下 两 种 : 

(1) 在 命令 管理 器 中 直接 编辑 Python 脚本 。 

在 当前 Python 提示 符 “>>>” 的 右 侧 输入 Python 脚本 ， 例 如 : 


print("hello world !") | 
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按 Enter 键 ， 运 行 结果 如 图 2.1 所 示 。 


Ml CAWindoswcoystem32 vnd exe - python 一 


图 2.1 在 命令 管理 器 中 直接 编辑 Python 脚本 


(2) 交互 式 运 行 Python 脚本 。 

交互 式 运 行 Python 脚本 ， 即 在 命令 管理 器 中 直接 打开 Python 脚本 。 打 开 命 令 管 理 器 后 ， 输 
入 Python 完整 文件 名 (包括 路 径 ) 即 可 。 例如 输入 “Python D: V helloworld” 并 按 Enter 键 ， 运 
行 结果 如 图 2.2 所 示 。 


2.2 交互 式 运行 Python 脚本 


2. Python 自 带 的 IDLE 

在 安装 Python 后 , 会 自动 安装 一 个 IDLE。 它 是 一 个 Python Shell， 可 以 与 Python 进行 交互 。 
在 所 有 程序 目录 中 ， 可 以 在 “Python3.7” 文 件 夹 中 找到 “IDLE (Python 3.7 64-bib”， 单 击 即 可 打 
F IDLE 窗口 。 通 过 IDLE 运行 Python 脚本 的 方式 有 以 下 两 种 : 

(1) 在 IDLE 中 直接 编辑 Python 脚本 。 

打开 IDLE 窗口 后 ,在 Python 提示 符 “>>>” 的 右 侧 直接 输入 Python 脚本 ， 同 在 命令 管理 
器 中 直接 编辑 Python 脚本 一 样 ， 如 图 2.3 所 示 。 


| File Edit Shell Debug Options Window Help. 


| 
Python 3.7.4 (tags/v3. 7. 4:009369112e, Jul 8 2019, 20:34:20) [MSC v.1916 64 bit (AMD64)] | 
1n32 | 
Type Sa “copyright”. “credits” or "license()" for more information. 
tC hello world X} 


he] hello vorld ! 


im 5 Cok4| 


图 2.3 在 IDLE 中 直接 编辑 Python 脚本 


(2) 在 IDLE 中 创建 Python 脚本 文件 。 
在 IDLE 主 窗口 的 菜单 栏 上 选择 File > New Eile， 将 打开 一 个 新 窗口 ， 在 该 窗口 中 ， 可 以 直 
接 编 写 Python 代码 ， 如 图 2.4 所 示 。 
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标题 栏 , Untitled 表示 未 命名 
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3. 集成 开发 环境 

集成 开发 环境 Integrated Development Environment, IDE) 是 用 于 提供 程序 开发 环境 的 应 用 
程序 ， 一 般 包括 代码 编辑 器 、 编 译 器 、 调 试 器 和 图 形 用 户 界 面 等 工具 。Python 程序 设计 常用 的 
集成 开发 工具 有 Microsoft Visual Studio、PyCharm， 以 及 Eclipse+PyDev。 本 书 以 PyCharm 为 例 ， 
进行 简单 介绍 。 

在 PyCharm 中 ， 选 择 File — New Project， 在 创建 新 项 目 窗口 中 设置 项 目 位 置 和 解释 器 ， 如 
图 2.5 所 示 。 
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图 2.5 创建 Python 项 目 


在 项 目 管理 器 中 选中 项 目 ， 右 击 ， 在 弹出 的 快捷 菜单 中 选择 New — Python File。 在 新 建 
Python File 窗口 中 的 文件 名 称 输入 框 中 输入 “hello.py”。 单 击 OK 按钮 ， 在 代码 窗口 中 输入 代码 
后 ， 选 择 Run — Run， 运 行 hello world.py， 如 图 2.6 所 示 。 

在 编写 Python 时 ， 当 使 用 中 文 输出 或 注释 时 运行 脚本 , 会 提示 错误 信息 : SyntaxError: Non- 
ASCII character "\x*…。 出 错 的 原因 是 Python 的 默认 编码 文件 是 ASCII 码 ， 而 Python 文件 中 使 
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用 了 中 文 等 非 英语 字符 ， 此 时 需要 在 Python 源 文件 的 最 开始 一 行 加 入 一 句 : 


#coding = UTF-8 


t sya; print Python We on Me’ (sys versian ays: platform) 


2.6 在 PyCharm 中 运行 Python 程序 


2.1.2 Python 程序 书写 格式 


Python 程序 书写 最 具 特 色 的 就 是 代码 缩 进 。Python 不 同 于 其 他 程序 语言 (如 C、Java 语言 )， 
采用 大 括号 “{ }” 分 隔 代码 块 ， 而 是 采用 代码 缩 进 和 冒号 和 ”区 分 代码 之 间 的 层次 关系 。 例 如 : 
判断 数字 a Al 的 大 小 关系 。 

Python 代码 : 


if ax<b: 

print ("a<b") 
else: 

print ("a>b") 


C 语言 代码 : 


#include <stdio.h> 
int main( ){ 


int a=6; 
int b=9; 
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if (a<b) { 

print ("a<b") ; 
} 
else{ 

print ("a>b") ; 
} 


此 外 ，Python 45 C. Java SiR SAA, Python 程序 不 需要 从 主 函 数 执行 ， 本 例 中 ， 直 接 执 
行程 序 第 一 条 语句 a=6。 证 语句 后 面 的 冒号 “: ”表示 下 一 行 是 子 模块 的 开始 ， 所 有 满足 证 条 件 
而 执行 的 语句 《代码 块 ) 缩 进 相同 。 

iE: Python 对 代码 缩 进 要 求 非常 严格 ， 同 一 个 级 别 代 码 块 的 缩 进 量 必须 相同 。 如 果 不 采 用 
代码 缩 进 ， 将 抛 出 SytaxError 异常 。 


2.1.3 ”变量 、 数 据 类 型 、 表 达 式 


1. 变量 

对 于 程序 语句 x1=2020，x1l 为 变量 ，2020 为 常数 ，= 为 赋值 操作 符 ， 语 句 将 等 号 右边 的 值 
赋 给 等 号 左边 的 变量 。 

变量 相当 于 计算 机 中 存在 的 一 个 位 置 ， 在 程序 运行 过 程 中 可 以 向 该 位 置 放 入 或 取出 数据 。 

语句 x2=x1+1 执行 时 ， 就 是 把 变量 xl 中 的 数据 取出 来 ， 加 上 1 后 ， 再 放 入 变量 x2 中 。 

标识 变量 需要 为 每 个 变量 起 一 个 名 字 ， 变 量 名 遵从 Python 标识 符 命名 规则 : 

(1) 由 字母 、 数 字 、 下 画 线 组 成 。 所 有 标识 符 可 以 包括 英文 、 数 字 以 及 下 画 线 “_”， 但 不 能 
以 数字 开头 。 

(2) 区 分 大 小 写 。 

(3) 不 能 使 用 Python 中 的 保留 字 。 

(4) 以 单 下 画 线 或 双 下 画 线 开 头 的 标识 符 有 特殊 意义 。 例 如 ，_init_() 代表 类 的 构造 函数 。 

在 Python 中, 变量 使 用 时 不 需要 像 C 语 言 那样 必须 先 声 明 , 而 是 可 以 直接 使 用 。 此 外 , Python 
是 一 种 动态 类 型 的 语言 ， 其 变量 的 类 型 也 可 以 随时 变化 。 

2. 数据 类 型 

在 数学 中 , 可 以 将 数字 分 为 整数 、 实 数 等 类 型 。 在 Python 中 ,数据 也 是 有 类 型 的 ， 因 此 , f£ 
放 数 据 的 变量 也 是 有 类 型 的 。 

CD 数值 型 。 包 括 整 数 类 型 和 浮 点 类 型 。 在 Python 3.0 之 后 的 版 本 中 ， 整 数 没有 大 小 限制 。 
1 是 整数 ， 为 整数 类 型 。1.0 是 实数 ， 为 浮 点 类 型 。 赋 值 语句 x=1 执行 后 ， 变 量 x 的 类 型 为 整数 
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类 型 。 

(2) 布尔 类 型 。 在 逻辑 学 中 ， 对 于 一 个 问题 可 以 用 “ 真 ” 或 “ 假 ” 描 述 ， 在 Python 中， 用 
True 表示 真 ， 用 False 表示 假 。 例 如 : 100>101 的 结果 为 “ 假 ” 赋值 语句 b=100>101 执行 后 ， 变 
T b 的 值 为 False。 

(3) 字符 串 类 型 。 字 符 串 是 字符 的 序列 。 在 Python 中 有 多 种 方式 表示 字符 串 , 通常 使 用 单 引 
号 、 双 引号 将 字符 序列 括 起 来 。 这 两 种 形式 本 质 上 没有 任何 差别 ， 只 是 在 形式 上 略 有 差异 。print 
"hello world" 中 ， 采 用 了 双 引 号 来 表示 字符 串 类 型 ， 同 样 ，'hello world' 也 可 以 表示 字符 串 类 型 。 

如 果 字 符 串 中 出 现 单 引号 或 双 引号 自身 ， 需 要 用 转 义 字符 “\” 将 单 引 号 或 双 引 号 进行 转 
义 。 例 如 : 在 执行 print ('Roban's functions’) 语句 时 ，Python 无 法 判定 book 后 面 的 单 引 号 
是 字符 串 的 结尾 ， 还 是 字符 串 中 的 符号 ， 在 执行 时 会 报错 。 此 时 ， 需 要 对 该 单 引 号 进行 转 义 : 
print ('RobanVs functions*)。 双 引号 表示 的 字符 串 中 出 现 的 单 引 号 不 需要 转 义 ， 例 如 : print 
(' 'Roban Vs functions ' ')。 

此 外 ，Python 中 还 可 以 通过 三 引号 将 字符 序列 括 起 来 ， 表 示 多 行 字符 串 。 

3. 表达 式 

表达 式 是 对 相同 类 型 的 数据 〈 如 常数 、 变 量 等 》 用 运算 符号 按 一 定 的 规则 连接 起 来 的 有 意 
MAIS 

COD 算术 运算 符 。 算 术 运算 符 是 处 理 四 则 混合 运算 的 符号 ， 常 用 于 数字 的 处 理 。 常 见 的 算 
术 运 算 符 如 表 2.1 Bre 
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说 明 


ET 
ee Uc CENE Ee] en A 
E. GRE -— MC A 
D NI 


取 整 除 ， 即 返回 商 的 整数 部 分 
求 余 ， 即 返回 除法 的 余数 12%8 
取 负 数 ， 即 返回 其 负数 -8 


(2) 比较 运算 符 。 比 较 运 算 符 也 称 关系 运算 符 ， 用 于 对 变量 或 者 表达 式 的 结果 进行 大 小 、 真 
假 等 比较 , 常用 于 条 件 语 句 中 作为 判断 的 依据 。Python 中 使 用 的 比较 运算 符 包括 < >. <=, >=. 
==、!=， 分 别 为 小 于 、 大 于 、 小 于 或 等 于 、 大 于 或 等 于 、 等 于 和 不 等 于 6 种 比较 运算 符 ， 如 表 
2.2 所 示 。 上 比较 运算 符 的 运算 结果 为 布尔 型 数据 。 如 果 比 较 结果 为 真 ， 则 返回 True; 如 果 为 假 
则 返回 False。 
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R22 算术 运算 符 
运算 符 说 明 实例 UT 
> i JE Un False 
« 水 平 156<225 True 
== ST grazie! True 
l= AEF 'a!lz tb! True 
>= 大 于 或 等 于 129>=156 False 
<= 小 于 或 等 于 : 108<=155 True 


(3) 逻辑 运算 符 . 逻辑 运算 符 是 对 真 和 假 两 种 布尔 值 进行 运算 , 运算 结果 仍 是 布尔 值 。Python 
中 的 逻辑 运算 符 主要 包括 and. GE 48.555. or OZR) M not OZE), WME 2.3 所 示 。 


表 2.3 逻辑 运算 符 


从 左 到 有 
从 左 到 右 
从 右 到 左 


运用 逻辑 运算 符 进 行 逻 辑 运算 时 ， 需 遵循 的 具体 规则 如 表 2.4 所 示 。 


表 2.4 运用 逻辑 运算 符 进行 逻辑 运算 的 规则 


表达 式 1 and 表达 式 2 表达 式 1 or 表达 式 2 not 表达 式 
False 
False 
False False True 
False True False True True 


(4) 运算 符 运 算 优先 级 。 在 表达 式 中 包括 多 种 运算 符 时 , 运算 优先 级 规则 为 : 算术 运算 符 高 
于 比较 运算 符 ， 比 较 运 算 符 高 于 逻辑 运算 符 。 : 

在 同类 运算 符 中 : 加 和 减 的 运算 优先 级 最 低 ,“ 非 ”高 于 “与 ”高 于 “或 ” 

例 : Python 中 的 各 种 运算 。 


c om 
" I 
o fF w 4 


aa 
MN 


print( a == c ) 
print(a» b) 
print( d 96 c ) 
print( d // b ) 
print( a - b) 
print( a*b + c*d ) 
print( a>d and a<b) 


print( a==c or a<b ) 


运行 结果 
True 
True 
0 
2 
1 
44 
False 
True 
2.1.4 ”条 件 语句 


条 件 语 句 ， 也 称 选择 语句 ， 即 根据 条 件 判断 的 结果 ， 执 行 不 同 的 程序 段 。 在 Python 中 ， 选 
择 语 名 主要 有 三 种 形式 ， 分 别 为 让 语句、 证 …else 语句 和 if--elif---else 多 分 支 语句 。 


1. if A 


Python 实现 分 支 结构 的 语句 主要 是 站 语句 ， 简 单 的 语法 格式 如 下 : 


if 表达 式 : 
语句 块 


其 中 ， 表 达 式 可 以 是 一 个 单纯 的 布尔 值 或 变量 ， 也 可 以 是 比较 表达 式 或 逻辑 表达 式 。 如 果 
表达 式 为 真 ， 则 执行 “语句 块 ”， 如 果 表 达 式 为 假 ， 则 跳 过 “语句 块 ”继续 执行 后 面 的 语句 。 


2. if---esle 语句 


if-else 语句 又 称 双 分 支 语句 ， 其 语法 格式 为 : 


让 表达 式 : 
语句 块 1 
else: 


语句 块 2 
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当 if-else 语句 中 表达 式 为 真 时 ， 执 行 让 后 面 的 语句 块 《 即 “语句 块 1”);， AM, HUT else 
后 面 的 语句 块 ( 即 “语句 块 2”)。 

Python SHE KDE AR, if Fl else 能 够 组 成 一 个 有 特定 逻辑 的 控制 结构 ， 有 相同 的 缩 进 。 
每 个 语句 块 中 的 语句 也 要 遵循 这 一 原则 。 

fi: 用 站 语句 判断 学 生 是 否 已 经 满足 入 学 年 龄 。 


age = 7 

if age >= 6: 
print ("满足 ") 

else: 


L print ("不 满足 ") 


利用 让 语句 判断 年 龄 是 否 为 6 以上， 如果 “是 ” 则 输出 “满足 ” 否则 ， 输 出 “不 满足 ”。 

Python 中 指定 任何 非 0 和 非 空 值 为 True，0 或 者 空 值 ( 如 空 的 列表 ) 为 False。 上 面 代码 中 ， 
如 果 条 件 age>=6 变 成 了 age， 程 序 在 执行 时 也 不 会 出 错 ， 而 是 执行 条 件 为 真 的 部 分 。 

3. 证 …elif…else 多 分 支 语句 

让 语句 可 以 实现 嵌 套 ， 即 在 站 语句 中 包含 让 语 句 。 

例 : 于 语句 判断 学 生 的 入 学 情况 。 


age = 10 
if age>=12: 
print ("初中 及 以 上 ") 
else: 
if age>=6: 
print ("小 学 ") 
else: 
print(" 未 入 学 ") 


上 面 这 段 代码 中 , 年龄 小 于 13 岁 的 都 属于 第 一 个 else 的 语句 块 ， 第 二 个 让 和 else 的 缩 进 与 
第 一 条 print 语句 相同 。 
Python 中 让 语句 也 可 以 实现 多 分 支 的 选择 ， 语 法 格式 为 : 
让 条 件 1: 
语句 块 1 
elif 条 件 2: 
语句 块 2 
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elif {F n: 
语句 块 n 
else: 
语句 块 n+l 
例 : 让 语句 判断 小 学 生 的 年 级 (分 为 7 个 等 级 )。 


age = 10 
if age>=11: 


print ("已 毕业 ") 
elif age>=10: 

print ("五 年 级 ") 
elif age>=9: 

print ("WER") 
elif age>=8: 

print ("三 年 级 ") 
elif age>=7: 

print ("二 年 级 ") 
elif age>6: 

print ("一 年 级 ") 
else: 


print ("AA") 


程序 运行 时 ， 首 先 判断 age>=11 是 否 为 真 ， 若 为 真 ， 则 输出 “已 毕业 ”结束 让 语句 ; 否则 ， 
继续 判断 其 他 表达 式 的 真 假 ， 如 果 最 终 进 入 else 的 语句 块 ， 那 么 表明 age<7， 输 出 “一 年 级 ”并 
退出 。 也 就 是 说 ， 让 语句 实现 了 6 个 分 支 ， 根 据 学生 的 年 龄 ， 准 确 判 断 其 所 处 的 年 级 。 


2.1.5 while 循环 语句 


Python 中 有 两 个 主要 的 循环 结构 ， 即 while 循环 和 for 循环 ， 用 于 在 满足 条 件 时 重复 执行 某 
段 代码 块 (循环 体 )， 以 处 理 需 要 重复 处 理 的 相同 任务 。 

1. while 循环 语句 

while 循环 是 通过 一 个 条 件 表达 式 来 控制 是 否 继 续 反复 执行 循环 体 中 的 语句 。 循环 体 指 一 组 
被 重复 执行 的 语句 。while 循环 语句 的 语法 格式 为 : 

while 表达 式 : 

循环 体 

Python 先 判 断 条 件 表达 式 的 值 为 真 或 假 , 如 果 为 真 , 则 执行 循环 体 中 的 语句 。 执行 完毕 , 会 

再 次 判断 条 件 的 值 为 真 或 假 ， 再 决定 是 否 执行 循环 体 中 的 语句 ， 直 到 条 件 表达 式 的 值 为 假 ， 退 
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出 循环 。 
Bil: while 循环 求 1+2+*…+100。 


sum=0 

i-1 

while i<=100: 
sum-sum*i 


i=it1 


|print sum 


程序 利用 sum 变量 保存 求 和 结果 ， 每 次 加 的 数 保 存在 变量 i 中 ,第 一 个 数 为 1 在 变量 i 小 
于 或 等 于 100 时 ，while 语句 条 件 为 真 ， 执 行 循环 体 ， 将 变量 i 值 加 到 sum， 为 了 再 次 执行 循环 
体 时 加 下 一 个 数 ， 需 要 将 变量 i 加 1， 循 环 体 语 句 执行 结束 后 ， 再 次 判断 条 件 是 否 为 真 ， 如 果 为 
真 再 次 执行 循环 体 。 当 条 件 不 满足 ， 即 =101 时 ， 循 环 结束 ， 输 出 sum 的 值 为 5050。 

对 于 有 限 循环 次 数 的 while 循环 程序 ， 为 确保 循环 能 够 正常 结束 ， 不 陷入 死 循环 〈 即 在 执行 
若干 次 循环 体 后 ，while 条 件 变 为 假 ， 循 环 结束 )， 循 环 体 中 一 定 要 包含 使 用 循环 条 件 变 为 假 的 
语句 ， 如 上 面 代码 中 的 i=i+1。 

2. for 循环 

for 循环 是 一 个 依次 重复 执行 的 循环 。 通常 适 用 于 枚 举 或 遍历 序列 ， 以 及 迭代 对 象 中 的 元 素 。 
for 循环 语法 格式 为 : 

for 变量 in 遍历 对 象 ; 

循环 体 

执行 for 循环 时 ， 遍 历 对 象 中 的 每 个 元 素 都 会 赋值 给 变量 ， 然 后 为 每 个 元 素 执行 一 饥 循环 
体 。 变 量 的 作用 范围 是 for 所 在 的 循环 结构 。 

Bil: for 循环 求 1+2+…+100。 


sum = 0 
for i in range(101): 
sum += i 


print (sum) 


程序 利用 sum 变量 保存 求 和 结果 ， 通 过 for 循环 遍历 range(101) 中 的 数字 ， 即 1—100 中 的 
所 有 整数 ， 并 将 每 次 遍历 的 结果 加 到 sum 变量 ，range(101) 中 所 有 数字 的 遍历 结束 后 ， 循 环 结 
束 ， 输 出 sum 的 值 为 5050。 

上 述 代码 中 ,使 用 了 range) 函数 ， 该 函数 是 Python 内 置 的 函数 ， 用 于 生成 一 系列 连续 的 整 
数 ， 多 用 于 for 循环 中 。 其 语法 格式 为 : 
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range (起 始 数值 ， 终 止 数值 [， 步 长 ]) 

range() 函数 生成 从 起 始 数 值 到 终止 数值 (不 含 终止 数值 ) 间 的 数字 序列 。 步 长 参数 为 可 选 
项 ， 默 认 值 为 1。 例 如 : range(1,5) 得 到 的 数字 序列 为 1,2,3,4; range(2,11,2) 得 到 的 数字 序列 为 
2,4,6,8,10。 


2.1.6 continue 5 break 语句 
1. continue 语句 
continue 语句 在 循环 结构 中 执行 时 ， 将 会 立即 结束 本 次 循环 ， 开 始 下 一 轮 循环 ， 即 跳 过 循环 
体 中 在 continue 语句 之 后 的 所 有 语句 ， 继 续 下 一 轮 循环 。 
例 : while 循环 输出 2*i，i 不 是 3 的 倍数 。 
for i in range(10): 
if i 96 3 == 0: 


it+=i 


continue 
else: 
print (2*i) 


it=i 


输出 结果 : 248101416 

2. break 语句 

break 语句 在 循环 结构 中 执行 时 ， 将 会 跳出 循环 结构 ， 转 而 执行 循环 结构 后 的 语句 ， 即 不 管 
循环 条 件 是 否 为 假 ， 遇 到 break 语句 都 将 提前 结束 循环 。 

例 : FA for 循环 找 出 20 以 内 被 3 除 余 2 的 数 。 
for i in range(20): 

if i%3 == 2: 

print (i) 


输出 结果 : 258111417 
例 : 用 for 循环 找 出 20 以 内 第 一 个 被 3 除 余 2 的 数 。 


for i in range(20): 
if i%3 == 2: 
print (i) 
break 


输出 结果 : 2 
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列表 由 一 系列 按 特 定 顺 序 排 列 的 元 素 组 成 。 元 素 可 以 是 任何 类 型 的 变量 。 与 其 他 语言 中 的 
数组 不 同 ， 列 表 元 素 之 间 可 以 没有 任何 关系 ， 可 以 是 不 同 数据 类 型 。 

列表 包含 多 个 元 素 。 通 常 给 列表 指定 一 个 表示 复数 的 名 称 ， 如 letters、digits E names. 

在 Python 中 用 方 括号 “[]” 来 表示 列表 ， 并 用 逗号 来 分 隔 其 中 的 元 素 。 

fil: 列表 定义 ， 元 素 可 以 是 任何 类 型 。 


numbers = [1,2,3,4,5] 


letters = ["a","b","c","d","e"] 
anything -[1,"Python", True] 
print (numbers) 

print(letters) 

print(anything) 


输出 结果 为 : 
[1, 2, 3, 4, 5] 
tan A oS iet d^. e'] 


[1, 'Python', True] 


1. 访问 列表 元 素 
通过 下 标 ( 索 引 ) 访问 列表 元 素 ， 格 式 如 下 : 
列表 名 称 [索引 ] 
例 : 计算 某 同 学 5 门 功课 的 平均 成 绩 。 
grades=[89,78,72,92,101] 
sum=0 


i=0 


while i<len(grades) : 
sum-sum*grades [i] 


isiti 


print ("average grade:",sum/len(grades) ) 


本 例 中 ， 利 用 函数 len0) 求 出 列表 长 度 ， 即 列表 元 素 个 数 ， 在 while 循环 中 ， 通 过 索引 (i 变 
E, 初 值 为 0) 访问 列表 元 素 ， 将 列表 元 素 的 内 容 累 加 求 和 ， 最 后 输出 平均 值 。 索 引 0 访问 的 是 
列表 的 第 一 个 元 素 ， 索 引 可 以 为 负数 ，--1 访问 的 是 列表 的 最 后 一 个 元 素 。 

与 C 语言 中 的 数组 一 样 ， 列 表 元 素 可 以 直接 赋值 修改 。 
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2. 操作 列表 常用 方法 
常用 操作 列表 的 方法 如 表 2.5 所 示 。 
表 2.5 中 示例 的 初始 列表 为 : 


numbers = [one, two, three, four, five] 


表 2.5 常用 操作 列表 的 方法 


示例 


append() 向 列表 尾部 添加 元 素 


xs.append("six") 结果 为 : 
xs=["one","two","three", 


"four","five","six"] 


insert(index, value) 


在 列表 中 插入 元 素 


删除 列表 末尾 的 元 素 ， 
popO 带 返回 值 ， 

实现 出 栈 操作 
del() 从 列表 中 删除 元 素 


pop(i) MERIK i 位 置 的 元 素 


根据 值 删除 元 素 
(多 个 满足 条 件 时 
只 删除 第 一 个 指定 的 值 ) 


remove(value) 


列表 进行 永久 性 排序 ， 
sort([reverse=True]) 

默认 为 升序 
sorted() 函数 对 列表 进行 临时 排序 


reverse() 


反 转 列表 元 素 的 排列 顺序 


len() 函数 


xs.insert(2,"six") 结果 为 : 


"on M "neret 


xs=["one","two","six", 


"three","four","five"] 
x2-xs.pop() 结果 为 : 
xs=["one","two", 
"three”,"four"], 
x2= "five" 
x2=xs.del(2), 


删除 第 2 个 元 素 


获取 列表 的 长 度 


x2=xs.pop(2), 
删除 第 2 个 元 素 并 返回 
x2="three" 


xs.insert(2,"five") 
xs.remove("five") 结果 为 : 


xs 与 初始 值 相同 


xs.sort() 结果 为 : 
xs=["five", "four", "one", 
"three", "two"] 
ys=sorted(xs) 结果 为 : 
xs 不 变 ，ys 为 xs 排序 结果 


xs.reverse() 结果 : 


xss|["five*, "four", ‘three’, 


'two', 'one'] 


len(xs) 


第 2 章 Python 编程 基础 i> 29 


3. 列表 分 片 

取 列 表 的 一 部 分 元 素 ， 称 为 分 片 。Python 对 列表 提供 了 强大 的 分 片 操作 ， 运 算 符 仍然 为 下 
标 运 算 符 。 创 建 列表 分 片 ， 需 要 指定 所 取 元 素 的 起 始 索引 和 终止 索引 ， 中 间 用 冒号 分 隔 。 分 片 
将 包含 从 起 始 索 引 到 终止 索引 《不 含 终止 索引 ) 对 应 的 所 有 元 素 。 

例如 ， 要 输出 列表 中 的 前 3 个 元 素 ， 需 要 指定 索引 0~3， 这 将 输出 分 别 为 0、1 和 2 的 元 素 。 


numbers = [1,2,3,4,5,6,7,8,9,0] 


print (numbers[0:3]) 


输出 结果 为 ， 


[1,525.31 


不 指定 起 始 索引 ，Python 将 自动 从 列表 头 开始 ; 不 指定 终止 索引 ，Python 将 提取 到 列表 末 
Fé; 终止 索引 小 于 或 等 于 起 始 索引 时 ， 分 片 结果 为 空 ， 两 个 索引 都 不 指定 时 ， 将 复制 整个 列表 。 


Pil: 复制 列表 。 


fruits=['apple','banana', 'watermelon','grape','lemon'] 
copyfruits=fruits[:] 
copyfruits.append('mango') 


print (fruits) 


print (copyfruits) 


输出 结果 为 : 
['apple', 'banana', 'watermelon', 'grape', 'lemon'] 
['apple', 'banana', 'watermelon', 'grape', 'lemon', 'mango'] 


4. 列表 的 加 和 乘 运算 
对 于 两 个 列表 ， 加 法 表示 连接 操作 ， 即 将 两 个 列表 合并 成 一 个 列表 。 例 如 : 
letters=['a','b','c','d','e''f'], numbers=[1,2,3,4,5,6,7,8,9,10], L =letters + numbers 
则 
ela Tb er aie te Mik, o3 4, 5.604879. 10] 
列表 的 乘法 表示 将 原来 的 列表 重复 多 次 。 例 如 L=[0]*100 会 产生 一 个 含有 100 个 0 的 列表 。 
乘法 操作 通常 用 于 对 一 个 具有 足够 长 度 的 列表 初始 化 。 


Meo oeil seat RR 


.1:8” 元 组 与 字典 


1. 元 组 

列表 适用 于 存储 在 程序 运行 期 间 可 能 变化 的 数据 集 ， 列 表 元 素 是 可 以 修改 的 。 在 需要 创建 
一 系列 不 可 修改 的 元 素 时 ， 可 以 使 用 元 组 。Python 将 不 能 修改 的 、 不 可 变 的 列表 称 为 元 组 。 

元 组 看 起 来 犹如 列表 ， 但 使 用 圆 括号 而 不 是 方 插 号 来 标识 。 定 义 元 组 后 ， 就 可 以 使 用 索引 
来 访问 其 元 素 ， 就 像 访问 列表 元 素 一 样 。 

fi: 元 组 的 使 用 。 
letterge('a!,'b!,'c!,'d', ell He") 
L=len(letters) 


for i in range(0,L): 
print (letters [i]) 


for a in letters: 


print (a) 


2. 字典 

字典 是 一 系列 “ 键 ; 值 ” 对。 每 个 键 都 与 一 个 值 相关 联 ， 键 和 值 之 间 用 冒号 分 隔 。Python 使 
用 键 来 访问 与 之 相关 联 的 值 。 与 键 相关 联 的 值 可 以 是 数字 、 字 符 串 、 列 表 乃 至 字典 。 

在 Python 中 ， 字 典 用 放 在 花 括号 中 的 一 系列 “ 键 : 值 ”对 表示 ， 各 个 “ 键 : 值 ” 对 之 间 用 
Wath. Ban: 

person_0="name": "LiMing","age": 24 

字典 变量 person_0 定义 了 name 和 age 两 个 键 ， 分 别 取 值 为 "LiMing " 和 24. 

访问 字典 元 素 与 访问 列表 元 素 类 似 ， 由 于 每 个 值 对 应 一 个 键 ， 访 问 该 值 时 需要 用 键 作 为 索 
引 。 例 如 ，person_0["age"] 可 以 得 到 "age" 键 对 应 的 值 24。 

字典 元 素 的 修改 、 添 加 与 删除 说 明 如 下 : 

(D 修改 : 对 已 有 的 键 直 接 赋 值 。 

(2) 添加 : 增加 新 的 “ 键 : 值 ” 对 ， 对 新 增加 的 键 赋值 。 

(3) ABR: 用 del 命令 删除 一 个 字典 键 。 

fil: 字典 元 素 的 修改 、 添 加 与 删除 。 


person={"name":"LiMing","age":24} 


print (person) 
print (person["name"] ) 


person["weight"]=120 


print (person) 
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person["name"]="LiuPing" 
print (person) 


del person["name"] 


print(person) 程序 运行 结果 为 : 


{'name': 'LiMing', 'age': 24} 
LiMing 


{'age': 24, 'weight': 120} 


{'name': 'LiMing', 'age': 24, 'weight': 120} 
Í'name': 'LiuPing', 'age': 24, 'weight': 120} 


字典 对 和 象 提供 了 items()、keys() 和 values 方法 ， 分 别 用 于 获取 “ 键 : 值 ”对 的 集合 、 键 的 


集合 和 值 的 集合 。 
Bl: 字典 的 遍历 。 


person-í"name":"LiMing","age":24) 

for k in person.keys(): 
print(k) 

for v in person.values(): 
print(v) 


for key,value in person.items(): 


print(key,value) 


items) 方法 取得 字典 中 “ 键 : 值 ”对 的 集合 ， 在 循环 中 分 别 赋值 给 key 变量 和 value 变量 。 


程序 运行 结果 为 : 


name age 
LiMing 24 
name LiMing 
age 24 


2.2 Python 函数 


函数 ， 最 早 由 中 国清 朝 数学 家 李 善 兰 function 翻译 而 来 ， 出 于 其 著作 《代数 学 》。 之 所 以 这 
么 翻译 ， 他 给 出 的 原因 是 “ 凡 此 变数 中 函 彼 变数 者 ， 则 此 为 彼 之 函数 ”也 即 函 数 指 一 个 量 随 着 


另 一 个 量 的 变化 而 变化 ， 或 者 说 一 个 量 中 包含 另 一 个 量 。 
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在 数学 中 ， 计 算 角 度 值 通常 会 用 到 arctane. arcsine 以 及 arccosz 等 函数 ， 根 据 参 数 x 的 值 ， 
即 可 求解 角度 值 。 求 解 得 到 的 角度 值 ， 可 以 作为 返回 值 ， 用 于 其 他 运算 。 如 : 求解 arctanz+y 时 ， 
可 以 定义 一 个 一 元 函数 ， 即 只 有 一 个 参数 的 函数 jz)=arctanz， 计 算 arctanz 后 得 到 一 个 值 ， 作 
为 函数 的 返回 值 ， 赋 值 给 f(z)。 通 过 f(x) 十 yy 即 可 表示 上 述 运 算 ， 对 于 f(x) 运算 ， 将 会 调用 
jz)=arctanz。 上 述 运算 中 运用 到 函数 的 参数 、 函 数 的 返回 值 、 函 数 的 定义 及 调用 。 

Python 函数 与 数学 函数 的 概念 是 相似 的 ， 除 了 具备 参数 、 返 回 值 外 ， 也 可 以 重复 调用 已 定 
义 的 函数 。 


2.2.1 RAVEN 


函数 定义 即 创建 函数 ， 可 以 理解 为 创建 一 个 具有 某 种 用 途 的 工具 。 函 数 定义 的 语法 格式 : 
def 函数 名 ([ 参 数 1， 参 数 2，…]): 
函数 体 
如 果 函 数 有 返回 值 ， 函 数 体 中 使 用 return 作 返 回 。return 关键 字 后 面 可 以 是 数值 或 其 他 类 
型 的 数据 ， 也 可 以 是 变量 或 表达 式 。 在 执行 到 return 语句 时 函数 结束 。 一 个 函数 可 能 会 有 多 个 
return 语句 。 


例 : 定义 可 以 实现 x x y 的 函数 f(z,y)， 并 计算 3 x 4 十 8 的 结果 。 


def f(x,y): 
return x*y 


print (£(3,4)+8) 


函数 代码 块 以 def 关键 词 开头 ， 后 接 函 数 标识 符 名 称 和 圆 括号 “0”， 括 号 里 面 是 函数 的 参 
数 ， 冒 号 后 面 对 应 缩 进 的 代码 块 是 函数 体 。 函 数 如 果 需 要 有 返回 的 结果 ， 利 用 return 关键 字 作 
返回 。 在 print 语句 调用 函数 f(xy) 时 ， 参 数 3 传 给 x，4 传 给 y， 计 算出 结果 12 后 ， 将 12 作为 
函数 返回 值 与 8 相 加 。 程 序 运 行 结果 是 输出 20。 

上 例 中 函数 定义 时 并 不 会 执行 ， 程 序 第 一 条 执行 的 语句 是 print 语句 ， 函 数 定义 中 的 语句 只 
有 在 被 调用 时 才 会 执行 。 

例 : 定义 对 列表 中 元 素 求 和 的 函数 ， 并 计算 range(10) 中 的 所 有 元 素 之 和 。 


def total(list): 


sum = 0 

for ain ats 
sum += i 

return sum 


array = range(10) 


s = total (array) 
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print (s) | 


2.2.2 ”函数 参数 


1. Python 变量 

在 C 语言 中 ， 系 统 会 为 每 个 变量 分 配 内 存 空间 ， 当 改变 变量 的 值 时 ， 改 变 的 是 内 存 空间 中 
的 值 ， 变 量 的 地 址 是 不 改变 的 。Python 采用 的 是 基于 值 的 管理 方式 。 

当 给 变量 赋值 时 ， 系 统 会 为 这 个 值 分 配 内 存 空间 ， 然 后 让 这 个 变量 指向 这 个 值 ， 当 改变 变 
量 的 值 时 ， 系 统 会 为 这 个 新 的 值 分 配 另 一 个 内 存 空间 ， 然 后 还 是 让 这 个 变量 指向 这 个 新 值 。 

如 果 没 有 任何 变量 指向 内 存 空间 的 某 个 值 , 这 个 值 称 为 垃圾 数据 , 系统 会 自动 将 其 删除 , 回 
收 它 占 用 的 内 存 空间 。 

在 Python 中 ， 可 以 使 用 id0 函数 获取 变量 或 值 的 地 址 。 

例 : Python 变量 与 地 址 。 


hie - number - 2048 
print("address of Num is :",id(Num)) 


print("address of number is :",id(number)) 


程序 运行 结果 为 : 
address of Num is : 2427449135056 
ex of number is :2427449135056 v 

数值 、 字 符 串 、 元 组 等 常量 对 象 的 存储 位 置 用 地 址 来 描述 ， 数 值 、 字 符 串 变量 指向 的 是 数 
值 或 字符 串 对 象 的 地 址 。Num 变量 与 number 变量 的 取 值 都 为 2048， 两 个 变量 都 指向 数值 对 象 
2048 所 在 的 地 址 ， 因 此 id(Num) 与 id(number) 相等 。 而 列表 、 字 典 变量 指向 的 存储 地 址 ， 在 修 
改 部 分 元 素 时 并 不 会 发 生变 化 ， 只 有 在 重新 定义 列表 时 ， 地 址 才 会 发 生变 化 。 

从 变量 指向 地 址 内 容 是 否 可 以 变化 的 角度 看 ， 数 值 、 字 符 串 、 元 组 是 不 可 变 类 型 ， 而 列表 
和 字典 则 是 可 变 类 型 。 

2. Python 函数 的 参数 传递 

在 数值 、 字 符 串 、 元 组 变量 作为 函数 参数 时 ， 如 fun(a)， 传 递 的 只 是 a 的 值 ， 不 会 影响 a 变 
量 本 身 。 如 果 在 函数 中 修改 a 的 值 ， 只 是 修改 另 一 个 复制 的 对 象 ， 不 会 影响 a 本 身 。 

在 列表 、 字 典 变量 作为 函数 参数 时 ， 则 是 将 列表 地 址 传 过 去 ， 如 果 在 函数 中 修改 列表 内 容 ， 
函数 外 部 的 列表 值 也 会 发 生变 化 。 

例 : 参数 传递 。 


mci ee 


def swap(x,y): 


EFF 
zay 
yomt 
def swaplist(x): 
t - x[0] 
x[0] = x[1] 
x[1] =t 
n-2 
m «3 
array = [2,3] 
swap(n,m) 
swaplist (array) 


print("n-",n," m=",m) 


print (array) 


程序 运行 结果 为 : 


a= 2 b= 3 
[3. 2] 


3. 缺 省 参数 
调用 函数 时 ， 缺 省 参数 的 值 如 果 没 有 传 入 ， 则 被 认为 是 默认 值 。 
Pil: 缺 省 参数 。 


def Student(name,grade = 3): 

print ("name:",name,"grade;", grade) 
Student (grade = 5,name = "LiMing") 
Student (name = "LiMing") 
Student ("LiMing") 


程序 运行 结果 为 : 


name: LiMing grade: 5 
name: LiMing grade: 3 


name: LiMing grade: 3 
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2.2.3 ”Python 模块 


在 Python 中， 可 以 将 一 组 相关 的 函数 、 数 据 放 在 一 个 以 .py 作为 文件 扩展 名 的 Python 文件 
中 ， 这 种 文件 称 为 模块 。Python 模块 为 函数 和 数据 创建 了 一 个 以 模块 名 称 命名 的 作用 域 。 利 用 
模块 可 以 定义 函数 、 类 和 变量 ， 模 块 里 也 可 以 包含 可 执行 的 代码 。Python 的 模块 机 制 应 用 于 系 
统 模块 、 自 定义 模块 和 第 三 方 模块 。 

模块 定义 好 后 ， 可 以 使 用 如 下 两 种 方式 引入 模块 。 

C1) 使 用 import 语句 引入 模块 。 语 法 如 下 : 

import modulel[，module2[，…moduleN] 

解释 器 过 到 import 语句 ， 如 果 模 块 位 于 当前 的 搜索 路 径 ， 该 模块 就 会 被 自动 导入 。 

调用 模块 中 函数 时 ， 格 式 为 : 

模块 名 . 函数 名 

在 调用 模块 中 的 函数 时 ， 之 所 以 要 加 上 模块 名 ， 是 因为 在 多 个 模块 中 可 能 存在 名 称 相同 的 
函数 ， 如 果 只 通过 函数 名 来 调用 ， 解 释 器 无 法 知道 到 底 要 调用 哪个 函数 。 

例 : 引入 系统 math 模块 ， 求 解 一 元 二 次 方程 。 


import math 


print("please input a,b,c") 

a=int (input ("a=")) 

b=int (input ("b=") ) 

c=int (input ("c=") ) 

deta-b**2-4*a*c 

if deta»-0: 
print ("xi=", (-b+math.sqrt (deta) ) /2/a) 
print ("x2=", (-b-math.sqrt (deta) )/2/a) 

else: 


| print("no result") 


input() 函数 用 于 键盘 输入 ， 返 回 值 类 型 为 字符 串 ， 由 于 一 元 二 次 方程 系数 为 整数 ， 需 要 利 
用 int0 函数 将 输入 的 字符 串 转换 为 整数 。 开 平方 函数 sqrt 不 属于 Python 系统 基本 函数 ， 位 于 
math 模块 中 ， 在 调用 该 函数 前 需要 导入 math 模块 。 

例 : course 模块 (文件 名 : course.py)。 


def information() : 


title= input( "input title of course :" ) 


time- float(input ("input time of course: " )) 
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print("title=", title) 


print("time= ", time) 


course 模块 定义 了 一 个 名 为 information 的 函数 ， 并 在 函数 中 声明 了 title. time 两 个 变量 分 
别 用 于 存储 课程 名 称 与 对 应 的 课时 ， 并 通过 input0 函数 与 print0 函数 输入 和 输出 课程 信息 。 
例 :， 主 程序 (文件 名 ; coursemain.py)。 


import course 
def main(): 
course.information() 


main() 


运行 程序 .coursemain.py， 结 果 为 : 


input title of course :Python 
input time of course: 72 
title= python 

time= 72.0 


(2) 使 用 from 语句 导入 指定 函数 。 

有 时 只 需要 用 到 模块 中 的 某 个 函数 ，from 语句 可 从 模块 中 导入 指定 的 部 分 。 格 式 如 下 : 
from 模块 名 import 函数 1[， 函 数 2[，… 函 数 n]] 

例如 : 

from math import sqrt 

如 果 想 把 一 个 模块 的 所 有 内 容 全 都 导入 ， 格 式 为 : 

from 模块 名 import * 


2.3 Python 对 象 与 类 


Python 中 的 任何 数据 都 是 对 象 。 例 如 ， 整 型 、 字 符 串 、 列 表 等 都 是 对 象 。 

每 个 对 象 由 标识 、 类 型 和 值 3 部 分 组 成 。 对 象 的 标识 〈 变 量 名 ) 代表 该 对 象 在 内 存 中 的 存 
储 位 置 。 对 象 的 类 型 表明 它 可 以 拥有 的 数据 和 值 的 类 型 。 在 Python 中 ， 可 变 类 型 的 值 是 可 以 更 
改 的 ， 不 可 变 类 型 的 值 是 不 能 修改 的 。 

对 象 不 仅 有 值 ， 还 有 相关 联 的 方法 。 例 如 ,一 个 字符 串 不 仅 包含 文本 ， 也 有 关联 的 方法 ,如 
将 整个 字符 串 变 成 小 写 或 者 大 写 的 lower) 方法 和 upper) 方法 。 

Bl: 对 象 的 类 型 与 方法 。 
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fruit="apple" 
number=23 

print (type(fruit) ) 
print (type (number) ) 


print (fruit .lower()) 


print (fruit .upper ()) 


type() 函数 的 作用 是 获取 变量 的 类 型 。 
程序 运行 结果 为 : 


<type 'str'> 


«type 'int'» 


apple 


APPLE 


任何 一 个 字符 串 对 象 都 有 lowerO 方法 和 upper 方法 ， 而 整 型 对 象 则 没有 这 两 种 方法 。 所 
有 的 字符 串 对 象 都 是 由 同一 个 模板 产生 的 ， 这 种 模板 用 于 描述 字符 串 对 象 的 共同 特征 ， 称 为 类 。 
对 象 是 根据 类 创建 的 ， 一 个 类 可 以 创建 多 个 对 和 象 。 

类 是 数据 (描述 事物 的 特征 ， 在 类 中 称 为 属性 ) 和 函数 (描述 事物 的 行为 ， 在 类 中 称 为 方 
法 ) 的 集合 。 

2.3.1 ”类 的 定义 与 使 用 

使 用 类 可 以 描述 任何 事物 。 下 面 通过 创建 一 个 简单 的 学 生 类 说 明 在 Python 中 类 的 定义 与 使 
用 方法 。 

fil; Animal 类 (文件 名 : animal.py). 
class Animal(): 


def __init__(self,kind,number): 


self .kind=kind 


self .number=number 
def printAnimal(self): 
print ("kind=",self.kind, "number=",self .number) 
a=Animal ("bird" ,53) 
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a.printAnimal () 
a.number=38 


print ("kind="a.kind, "number=",a.number) 


Animal 类 说 明 : 
(1) 在 Python 中 , 使 用 class 关键 字 来 声明 一 个 类 。 根据 约定 , 首 字 母 大 写 的 名 称 指 的 是 类 。 
类 定义 中 的 括号 指定 的 父 类 是 object， 表 示 从 普通 的 Python 对 象 类 创建 Animal 25. 
(2) __init__Q 方法 。 这 是 一 个 特殊 的 方法 ， 称 为 构造 方法 。 开 头 和 末尾 各 有 两 个 下 面 线 ， 
是 一 种 约定 ， 骨 在 避免 Python 默认 方法 与 普通 方法 发 生 名 称 冲突 。 
方法 中 包含 3 个 形式 参数 ，self、kind 和 number。 其 中 ， 形 式 参 数 self 必 不 可 少 ， 还 必须 位 
于 其 他 形式 参数 的 前 面 。Python 调用 __init__() 方法 创建 Student 实例 时 ， 将 自动 传 入 实际 参数 
self。 每 个 与 类 相关 联 的 方法 调用 都 自动 传递 实际 参数 self， 它 是 一 个 指向 实例 本 身 的 引用 ， 让 
实例 能 够 访问 类 中 的 属性 和 方法 。 
(3) 属性 。__init__0 方法 中 定义 的 两 个 变量 都 有 前 级 self。 以 self 为 前 缀 的 变量 都 可 供 类 
中 的 所 有 方法 使 用 ， 可 以 通过 类 的 任何 实例 来 访问 这 些 变 量 。self.kind=kind 获取 存储 在 形式 参 
数 kind 中 的 值 ， 并 将 其 存储 到 变量 kind H, 然后 该 变量 被 关联 到 当前 创建 的 实例 。self.number= 
number 的 作用 与 此 类 似 。 像 这 样 可 通过 实例 访问 的 变量 称 为 属性 。 
(4) 在 创建 Animal 实例 a IY, Python 将 调用 Animal 类 的 方法 — init 0。 由 于 self 自动 传 
递 ， 因 此 不 需要 在 参数 中 包括 self， 只 需 给 最 后 两 个 形式 参数 (kind 和 number) 提供 值 。 通 过 
将 实际 参数 bird 和 53 分 别传 递 给 形式 参数 kind 和 number, X kind 属性 和 number 属性 赋值 。 
(5) 类 中 定义 了 另外 一 个 方法 : printAnimal()。 由 于 方法 不 需要 额外 的 信息 ， 因 此 只 有 一 个 
形式 参数 self。 
(6) 使 用 点 号 “.” 操 作 符 访问 对 象 的 属性 和 方法 。 
CD) 可 以 通过 对 对 和 象 属性 直接 赋值 的 方式 修改 属性 或 增加 属性 。 
Animal 类 实例 的 输出 结果 : | 
kind= bird number= 53 


kind= bird number= 38 


2.3.2 ”类 的 继承 


编写 的 类 以 另 一 个 已 有 的 类 为 基础 ， 可 使 用 继承 。 一 个 类 继承 另 一 个 类 时 ， 它 将 自动 获得 
另 一 个 类 的 所 有 属性 和 方法 ， 原 有 的 类 称 为 父 类 ， 而 新 类 称 为 子 类 。 子 类 继承 了 父 类 的 所 有 属 
性 和 方法 ， 同 时 还 可 以 定义 自己 的 属性 和 方法 。 

fil: Pigeon 类 《〈 文 件 名 : animal.py)。 
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class Animal(object): 
def . init. (self,kind,number): 
self .kind=kind 
self .number=number 
def printAnimal (self): 
print ("kind=",self.kind, "number=",self.number) 
class Pigeon (Animal): . 
def | init. (self,kind,number,weight,color): 
super(Pigeon,self).. init. (kind,number) 
self.weight-weight 
self.color-color 
def printCharacter(self): 
print("weight-",self.weight,"color-",self.color) 
a- Pigeon ("Pigeon",23,1.2, "write") 
a.printAnimal () 


a.printCharacter() 


Pigeon 类 说 明 : 

(1) 创建 子 类 。 定 义 子 类 时 ， 必 须 在 括号 内 指定 父 类 的 名 称 。 

(2) super() 是 一 个 特殊 函数 ， 帮 助 Python 将 父 类 和 子 类 关联 起 来 。 这 行 代码 让 Python 调用 
Pigeon Æ (Animal) 的 方法 — init. () ik Pigeon 实例 包含 父 类 的 所 有 属性 。 父 类 也 称 为 超 
类 (superclass)， 名 称 super 因此 而 得 名 。 方 法 __init__( 定义 中 包含 5 个 形式 参数 : self、kind、 
number、weight 和 color。 其 中 ， 形 式 参数 self 必 不 可 少 。 由 于 Animal 类 在 构造 函数 中 创建 了 
kind 和 number 属性 ，Pigeon 类 将 继承 父 类 这 两 个 属性 。 父 类 中 不 包含 的 属性 由 子 类 在 构造 函数 
中 创建 。 

(3) 子 类 继承 了 父 类 方法 printAnimal()， 可 以 直接 调用 。 

程序 运行 结果 为 : 


kind=Pigeon number=23 


weight=1.2 color=write 
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文件 的 主要 作用 是 存储 数据 。 文 件 存储 在 磁盘 或 其 他 辅助 存储 设备 上 ;是 可 读 可 写 的 。 磁 
盘存 储 数 据 的 基本 单位 是 字 节 (8 位 二 进 制 数 )， 因 此 读 写 文件 的 基本 单位 是 字 节 。 文件 中 存储 
的 内 容 是 ASCH 字符 或 文字 ， 这 类 文件 称 为 文本 文件 。 文 件 中 存储 的 数据 是 整 型 (包括 其 他 表 
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示 成 无 符号 整数 的 数据 类 型 ， 如 图 像 、 音 频 或 视频 )、 浮 点 型 或 其 他 数据 结构 ， 这 类 文件 称 为 二 
进 制 文件 。 应 用 程序 在 处 理 文件 时 ， 可 以 根据 文件 存储 的 内 容 决定 读 写 方式 。 

读 写 文件 主要 有 两 种 方式 : 

(1) 顺序 读 写 : 每 个 数据 (字符 、 整 数 或 其 他 类 型 数据 ) 必须 按 顺 序 从 头 到 尾 一 个 接 一 个 地 
进行 读 写 。 进 行 顺序 读 写 时 ，Python 会 设置 一 个 变量 ， 用 于 存储 当前 要 读 写 数据 的 位 置 ， 每 次 
读 写 完成 后 ， 变 量 会 自动 增加 ， 指 向 下 一 个 数据 位 置 。 

(2) 随机 读 写 ， 读 写 文件 中 任意 位 置 的 数据 时 ， 可 以 直接 定位 到 该 位 置 进行 读 写 。 


2.4.1 ”文本 文件 读 写 


在 Python 中 ， 操 作文 件 需要 先 创 建 或 者 打开 指定 文件 并 创建 文件 对 象 ， 可 以 通过 open) FR 
数 实现 。open() 函数 包含 两 个 参数 ， 分 别 为 文件 名 和 文件 打开 模式 ， 其 中 文件 打开 模式 为 可 选 
参数 。 

YER: 如 果 不 指定 路 径 ，Python 将 在 当前 执行 的 文件 所 在 的 目录 中 查找 文件 。 函 数 open() 
返回 一 个 表示 文件 的 对 象 。 

1. 读 文本 文件 

在 读 取 文 本 文件 之 前 , 首先 在 DD 盘 创 建 一 个 名 为 “poem.txt” 的 文本 文件 。 在 Python H, % 
用 的 读 取 文 本 文件 的 方式 主要 有 以 下 两 种 。 

C1) 读 取 整 个 文本 文件 。 读 取 文 件 的 全 部 内 容 ， 调 用 read() 方法 即 可 实现 。 

例 : 读 取 整 个 文本 文件 。 


with open("D:\poem.txt") as f: 
content=f .read() 


print (content) 


上 述 例子 中 ，with 表示 在 不 需要 访问 文件 时 将 其 关闭 。 在 with 结构 中 ， 只 调用 了 open) 1T 
开 文 件 ， 并 没有 调用 close(), Python 会 在 合适 的 时 候 自 动 将 文件 关闭 。 

(2) 逐 行 读 取 。 在 使 用 read) 方法 读 取 文件 时 ， 如 果 文 件 过 大 ， 一 次 性 读 取 全 部 内 容 到 内 
存 ， 容 易 造 成 内 存 不 足 ， 所 以 通常 会 采用 逐 行 读 取 。 文 件 对 象 提供 了 readline) 方法 用 于 每 次 读 
取 一 行 数 据 。 

例 : 逐 行 读 取 文 件 。 


with open("D:\poem.txt") as f: 
for line in f: 


print (line) 
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for 语句 对 文件 对 象 执 行 循环 ， 遍 历 文件 中 的 每 行 。 在 循环 过 程 中 ，line 取 值 为 文本 文件 一 
THAR (包括 换行 符 )。 

程序 输出 结果 与 上 例 相 比 ， 各 行 间 多 出 一 个 空白 行 。 因 为 在 文本 文件 中 ， 每 行 的 末尾 都 有 
一 个 看 不 见 的 换行 符 ， 而 print 语 旬 也 会 加 上 一 个 换行 符 ， 因 此 每 行 末尾 都 有 两 个 换行 符 ， 一 个 
来 自 文件 ， 另 一 个 来 自 print 语句 。 要 消除 这 些 多 余 的 空白 行 ， 可 以 使 用 字符 串 对 象 的 rstrip() 77 
法 将 line 右 端 的 空白 符 去 掉 ， 如 : print(line.rstrip()). 

此 外 ， 文 件 对 象 中 还 提供 了 readlines() 方法 用 于 逐 行 读 取 文件 内 容 。 

Pil: 逐 行 读 取 文件 内 容 。 


with open("D:\poem.txt") as f: 


lines = f.readlines() 


print (lines) 


2. 写 文本 文件 

Python 的 文件 对 象 提 供 了 write) 方法 可 以 向 文件 中 写 入 内 容 ， 常 用 的 写 入 方式 分 别 为 以 写 
入 模式 写 文本 文件 和 以 附加 模式 写 文本 文件 。 

(OD 以 写 入 模式 写 文本 文件 。 要 将 文本 写 入 文件 ， 在 调用 open) 时 需要 提供 男 一 个 实际 参 
数 ， 操 作文 件 的 模式 告诉 Python 要 写 入 打开 的 文件 。 

fil: 以 写 入 模式 写 文本 文件 。 


with open("D:\poem.txt","w") as f: 
f.write(" 相 见 欢 ") 
f,write(" 无 言 独 上 西 楼 ， 月 如 钩 。") 
f.write(" S XE A ZR Po UR AK.) 
f,Wwrite(" 剪 不 断 ， 理 还 乱 ， 是 离愁 ， ") 
fwrite(" 别 是 一 般 滋味 在 心头 。") 


上 述 代 码 中 ， 调 用 open) 时 提供 了 两 个 实际 参数 ， 第 一 个 实际 参数 也 是 要 打开 文件 的 名 称 ， 
第 二 个 实际 参数 "w" 表示 以 写 入 模式 打开 这 个 文件 。 打 开 文件 时 ， 可 指定 读 取 模 式 "r"、 写 入 模 
式 "w"、 附 加 模式 "a" ,或 读 取 和 写 入 的 模式 "r+""w+"。 如 果 省 略 了 模式 实际 参数 ， Python 将 以 默 
认 的 只 读 模式 打开 文件 。 

如 果 写 入 的 文件 不 存在 ， 函 数 open) 将 自动 创建 文件 。 如 果 指 定 的 文件 已 经 存在 ，Python 
将 在 返回 文件 对 象 前 清空 该 文件 。 

文件 对 象 的 方法 write) 将 一 个 字符 串 写 入 文件 。 由 于 是 顺序 读 写 模式 ， 连 续 的 3 个 写 方 
法 将 3 个 字符 串 写 到 文本 文件 中 。 在 写 文件 的 过 程 中 ， 并 没有 写 入 换行 符 ， 因 此 文件 的 内 容 在 
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文本 编辑 器 中 只 显示 一 行 。 如 果 想 要 分 行 写 文件 ， 可 以 加 入 换行 符 “\n”， 如 : f.write(" 相 见 欢 
M15 

(2) 以 附加 模式 写 文本 文件 。 附 加 模式 是 指 打开 一 个 文件 用 于 追加 。 如 果 该 文件 已 存在 , 新 
的 内 容 将 会 被 写 入 已 有 内 容 之 后 。 如 果 该 文件 不 存在 ， 创 建新 文件 进行 写 入 。 

例 ; 以 附加 模式 写 入 文本 文件 。 


with open("D:\poem.txt","w") as f: 
f.write(" 浣溪沙 \n") 
f.write(" 一 曲 新 酒 就 一 杯 ， 去 年 天 气 旧 襄 台 \n") 
f.write(" 乡 阳 西 下 儿 时 回 ? Nn") 
with open("D:\poem.txt","a") as f: 
f.write(" 无 可 奈何 花 落 去 似曾相识 燕 归来 。\na") 
f.write(" 小 园 香 径 独 徘徊 。\n") 


2.4.2 ”二 进 制 文件 读 写 


与 文本 文件 的 读 写 一 样 ， 在 读 写 二 进 制 格式 的 文件 时 也 需要 先 打开 文件 ， 再 进行 文件 读 写 。 
打开 二 进 制 文件 时 ， 可 指定 读 取 模式 "tb"、 写 入 模式 "wb"、 附 加 模式 "ab"， 或 读 取 和 写 入 的 模 
式 "rb+""wb+"s 

由 于 Python 中 的 整数 、 浮 点 数 等 类 型 的 数据 都 是 对 象 ， 并 不 是 真正 写 入 文件 的 内 容 ， 因 此 
在 写 入 二 进 制 文件 之 前 , 需要 先 利用 struct 模块 的 pack() 方法 对 整数 等 类 型 数据 作 格式 转换 ,， 转 
换 方法 为 : 

struct.pack(fmt,values) 

pack() 方法 中 fmt 参数 定义 如 表 2.6 Bros. 


表 2.6 fmt 参数 定义 


Pe ye] 

integer d double 
ORT integer S char[] 
ae 


例如 : 写 三 进 制 文件 。 


C 
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import struct 
numbers = [0,1,2,3,4,5,6,7,8,9] 
filename="D:\list_number.dat" 
with open(filename,"wb") as f: 
for number in numbers: 
d-struct.pack("i",number) 


f.write(d) 


d=struct.pack("i" ,number) 


语句 struct.pack("i",number) 将 整数 转换 成 C 语言 的 整 型 格式 : 占 4 字 节 ， 低 位 在 前 ， 列 表 


numbers 的 整数 都 按 这 种 格式 写 入 文件 。 
Python 读 二 进 制 文件 时 ， 从 文件 中 读 到 一 组 字 节 序 列 ， 需 要 使 用 struct.unpack() 方法 将 其 转 


换 成 Python 的 数据 类 型 。 
例 : 读 二 进 制 文件 。 


import struct 


filename="D:\list_number.dat" 
sum=0 
with open(filename,"rb") as fr: 
for i in range(0,10): 
b-fr.read(4) 
d-struct.unpack("i",b) 


sum-sum + d[0] 


print('sum-',sum) 


程序 读 取 的 二 进 制 文件 为 上 例 生成 文件 。 文 件 对 象 的 read 方法 不 指定 参数 时 ， 读 取 的 是 
文件 的 全 部 内 容 ; 指定 数值 时 ， 读 取 指 定数 量 的 字 节 。 由 于 文件 中 用 4 字 节 存储 一 个 整数 ， 
此 read 方法 指定 的 参数 为 4。unpack() 方法 将 长 度 为 4 的 字符 ( 节 ) 转换 成 列表 ,列表 的 第 1 个 
元 素 即 为 读 取 的 整数 。 


243 异常 


Python 程序 执行 期 间 发 生 错 误 时 ， 程 序 将 停止 ， 并 显示 一 个 traceback， 其 中 包含 有 关 异 常 
的 报告 。Python 使 用 “异常 ”对 和 象 来 管理 程序 执行 期 间 发 生 的 错误 。 在 产生 错误 时 ，Python 会 
创建 一 个 异常 对 象 。 如 果 编 写 了 处 理 该 异常 的 代码 ， 程 序 将 继续 运行 。 

异常 是 使 用 try-except 代码 块 处 理 的 。try-except 代码 块 让 Python 执行 指定 的 操作 ， 同 时 告 
Vf Python 发 生 异 常 时 怎样 处 理 。 使 用 了 try-except 代码 块 时 ， 即 使 出 现 异常 ,程序 也 将 继续 运行 。 


agli il cs 


在 编写 程序 的 过 程 中 ， 经 常会 使 用 各 种 运算 符 ， 特 别 在 进行 除法 运算 时 ， 除 数 不 能 为 0， 这 
时 就 需要 进行 异常 处 理 。 


例 ， 除 数 为 0 的 异常 处 理 。 


try :! 


a = int(input(" 除 数 : ")) 
b = int(input ("被 除数 ; ")) 
print (a/b) 


except: 


print("You can't divide by zero !") 


except 代码 块 在 出 错时 会 执行 。Python 细 分 了 多 种 不 同类 型 的 错误 ， 如 IOError、ZeroDivi- 
sionError 等 。 如 果 进 一 步 限 定 出 错 情况 ， 在 except 关键 字 后 面 可 以 使 用 具体 的 错误 类 型 。 如 上 
例 中 使 用 except ZeroDivisionError 更 确切 。 

此 外 ， 对 文件 进行 操作 时 ， 常 会 遇 到 找 不 到 文件 的 问题 ， 如 要 查找 的 文件 可 能 在 其 他 地 方 、 


文件 名 可 能 不 正确 或 者 这 个 文件 根本 就 不 存在 。 对 于 所 有 这 些 情形 ， 都 可 使 用 try-except 代码 块 
以 直观 的 方式 进行 处 理 。 


filename = “animal.txt" 


try: 
with open(filename) as f: 
content-f.read() 


print (content) 


except IOError: 


print("the file "+filename +" does not exist.") 


ROS 使 用 概述 


CHAPTER 3 


Roban 机 器 人 基于 ROS 构建 ， 以 ROS 平台 为 基础 提供 了 Roban 的 应 用 API， 因 此 有 必要 在 
介绍 机 器 人 的 使 用 之 前 ， 先 对 ROS 做 一 个 简单 介绍 。 


3.1 ROS 简介 


ROS 的 官方 定义 如 下 : 

ROS 是 面向 机 器 人 的 开源 的 元 操作 系统 (meta-operating system) 。 它 能 够 提供 类 似 传统 操 
作 系统 的 诸多 功能 ， 如 硬件 抽象 、 底 层 设备 控制 、 常 用 功能 实现 、 进 程 间 消 息 传递 和 程序 包 管 
理 等 。 此 外 ， 它 还 提供 相关 工具 和 库 ， 用 于 获取 、 编译 、 编辑 代码 ， 以 及 在 多 个 计算 机 之 间 运 行 
程序 ， 完 成 分 布 式 计算 。 

这 个 官方 定义 强调 的 是 ROS 的 特殊 性 ， 但 是 没有 指出 ROS 可 以 给 机 器 人 软件 开发 带 来 哪 
些 便利 。 下 面 将 简要 说 明 ROS 在 机 器 人 软件 系统 中 可 以 发 挥 的 作用 。 

1. 分 布 式 计算 

在 现在 的 机 器 人 系统 中 ， 往 往 需 要 多 个 进程 协同 工作 。 例 如 ， 在 实际 机 器 人 的 使 用 过 程 中 ， 
传感器 的 数据 处 理 和 执行 器 的 控制 数据 发 送 往往 是 被 分 开 的 。 而 在 比较 复杂 的 机 器 人 系统 中 , 甚 
至 会 出 现 需要 多 台 计 算 机 在 一 个 机 器 人 上 协同 完成 同一 个 机 器 人 的 控制 任务 的 情况 。 例 如 ， 一 
台 视 觉 处 理 计算 机 专门 用 于 处 理 物 体 识 别 的 问题 ， 需 要 消耗 比较 大 的 算 力 ， 而 另外 一 台 用 于 实 
时 控制 的 计算 机 对 应 程序 运行 的 实时 性 要 求 很 高 ， 此 时 就 需要 两 台 计 算 机 协同 工作 ， 进 行进 程 
间 通 信 来 满足 实际 需要 。 再 比如 ， 在 机 器 人 使 用 的 过 程 中 常常 还 需要 涉及 人 机 交互 的 问题 ， 在 
人 机 交互 的 过 程 中 ,往往 需要 通过 笔记 本 计算 机 或 者 其 他 移动 设备 发 送 指令 来 控制 机 器 人 运动 。 
这 种 人 机 交互 接口 也 被 认为 是 机 器 人 软件 的 一 部 分 。 

ROS 中 为 了 解决 单 台 计算 机 或 者 多 台 计 算 机 之 间 的 多 进程 通信 和 问题， 提供 了 两 种 相对 简单 
但 完备 的 机 制 ， 在 后 文中 将 详细 讨论 。 
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2. 软件 复 用 问题 

随 着 机 器 人 研究 的 发 展 ， 出 现 了 许多 针对 导航 、 路 径 规划 、 建 图 等 通用 任务 的 算法 。 当 然 ， 
任何 一 种 算法 能 得 到 良好 实用 的 前 提 是 其 可 用 于 新 的 领域 且 具 有 良好 的 可 复 用 性 。 在 不 同系 统 
下 确保 同一 种 机 器 人 算法 的 正确 运行 也 不 是 一 个 容易 满足 的 问题 ，ROS 通过 两 种 方法 来 试图 解 
决 这 个 问题 ， 一 方面 ROS 标准 包 提供 许多 可 复 用 的 机 器 人 算法 实现 供 机 器 人 开发 者 使 用 ， 另 一 
方面 得 益 于 ROS 良好 的 生态 , 也 有 许多 开发 者 基于 ROS 的 接口 实现 了 很 多 开源 算法 与 软件 ， 以 
避免 开发 者 为 了 集成 不 同 算法 而 进行 的 重复 工作 。 

3. 快速 测试 

因为 机 器 人 开发 软件 比 开 发 其 他 通用 软件 的 复杂 度 高 ， 主 要 是 因为 调试 过 程 复杂 。 还 因为 
人 硬件 维修 、 经 费 有 限 等 因素 ， 不 一 定 随时 有 充足 的 机 器 人 可 供 使 用 。 为 了 方便 调试 ，ROS 提供 
了 不 同 的 方式 来 解决 调试 问题 。 一 方面 具有 良好 设计 的 ROS 可 以 将 底层 控制 框架 和 顶层 的 数据 
处 理 部 分 进行 分 离 ， 从 而 可 以 使 用 模拟 器 或 者 仿真 软件 来 蔡 代 实际 的 底层 硬件 模块 ， 进 而 可 以 
方便 地 独立 测试 算法 ， 提 高 测试 效率 。Roban 机 器 人 的 开发 也 采用 了 模块 化 的 方式 进行 了 分 离 ， 
从 而 可 以 单独 开发 上 层 部 分 ， 利 用 V-REP 仿真 环境 对 所 开发 代码 的 有 效 性 进行 验证 ， 使 得 程序 
的 开发 不 需要 依赖 实体 机 器 人 ， 也 不 用 担心 对 实体 机 器 人 造成 损坏 。 此 外 ，ROS 还 提供 了 名 为 
ROS. Bag 的 调试 工具 用 于 对 开发 和 使 用 过 程 中 的 数据 进行 记录 , 并 且 可 以 按照 时 间 戳 进行 回访 ， 
从 而 可 以 获取 更 多 的 机 器 人 测试 数据 ， 并 可 以 在 这 个 过 程 中 测试 不 同 的 数据 处 理 算法 对 比 效 果 。 
此 外 ，ROS 还 提供 了 多 种 机 制 用 于 机 器 人 的 调试 ， 如 rqt_graph 和 rqt plot 等 ， 这 些 工 具 都 可 以 
有 力 地 提高 开发 效率 。 


32 ”程序 包 与 节点 


ROS 中 ,所 有 软件 都 被 组 织 为 软件 包 的 形式 , Og ROS 软件 包 或 功能 包 ， 有 时 也 简称 为 包 。 
ROS 软件 包 是 一 组 用 于 实现 特定 功能 的 相关 文件 的 集合 ， 包 括 可 执行 文件 和 其 他 支持 文件 。 比 
如 Roban 机 器 人 中 的 BodyHub 节点 与 SensorHub 节点 就 是 典型 的 功能 包 。 


3.2.1 ”程序 包 与 节点 介绍 


ROS 可 以 使 用 一 些 特定 的 指令 来 和 指令 包 进 行 交 互 。 例 如 ”使 用 下 列 指 令 可 以 获取 所 有 已 
安装 的 软件 包 清 单 。 


rospack list 


在 Roban 机 器 人 中 运行 这 条 指令 可 以 得 到 很 多 软件 包 ， 这 里 截取 一 部 分 : 


ctexecpackage /home/lemon/robot_ros_application/catkin_ws/src/actexecpackage 


actionlib /opt/ros/kinetic/share/actionlib 
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actionlib_msgs /opt/ros/kinetic/share/actionlib_msgs 

actionlib_tutorials /opt/ros/kinetic/share/actionlib_tutorials 

angles /opt/ros/kinetic/share/angles 

async_web_server_cpp /home/lemon/robot_ros_application/catkin_ws/src/async_web_server 
-CPP 


beginner_tutorials /home/lemon/robot_ros_application/catkin_ws/src/beginner_tutorials 


bodyhub /home/lemon/robot_ros_application/catkin_ws/src/bodyhub 

bond /opt/ros/kinetic/share/bond ; 

bondcpp /opt/ros/kinetic/share/bondcpp 

bondpy /opt/ros/kinetic/share/bondpy 

camera_calibration /opt/ros/kinetic/share/camera_calibration 
camera_calibration_parsers /opt/ros/kinetic/share/camera_calibration_parsers 
camera_info_manager /opt/ros/kinetic/share/camera_info_manager 

catkin /opt/ros/kinetic/share/catkin 


class_loader /opt/ros/kinetic/share/class_loader 


要 找到 一 个 软件 包 的 目录 ， 使 用 rospack find 命令 : 


rospack find package-name | 


在 记 不 住 ROS 包 的 完整 名 称 时 ， 可 以 使 用 rospack fine 命令 ， 因 为 该 命令 支持 tab 命令 补 全 ， 
可 以 方便 地 找到 所 需 ROS 包 的 路 径 。 
查看 软件 包 : 要 查看 软件 包 目 录 下 的 文件 ， 使 用 如 下 命令 : 


rosls package-name 


3.2.2 ”节点 的 编译 与 运行 


ROS 的 一 个 基本 目标 是 使 开发 者 设计 的 很 多 称 为 节点 Code) 的 几乎 相对 独立 的 小 程序 能 
够 同时 运行 。 为 此 ， 这 些 节点 必须 能 够 彼此 通信 。ROS 中 实现 通信 的 关键 部 分 就 是 ROS 节点 管 
理 器 。 要 启动 节点 管理 器 (roscore)， 使 用 如 下 命令 : 


roscore 


在 Roban 机 器 人 中 ,默认 情况 下 该 管理 器 是 一 直 在 运行 的 .下 文中 也 会 介绍 一 个 名 为 roslaunch 
的 工具 , 它 可 以 一 次 启动 几 个 相关 的 节点 并 完成 配置 ,这 个 工具 如 果 发 现 不 存在 已 启动 的 roscore， 
会 自动 启动 一 个 供 节点 使 用 。 
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在 启动 了 roscore 之 后 就 可 以 运行 ROS 的 程序 了 ， 在 Roban 机 器 人 中 每 个 ROS 的 功能 包 中 
可 能 会 包含 一 个 或 者 多 个 节点 ， 特 别 需要 注意 的 是 如 果 同 一 个 程序 需要 同时 运行 多 个 副本 ， 则 
需要 保证 每 个 副本 的 节点 名 称 不 同 。 

1. 启动 节点 

启动 节点 的 基本 命令 是 rosrun， 启 动 器 使 用 方法 如 下 : 


rosrun package-name executable-name 


rosrun 命令 有 两 个 参数 ， 第 一 个 参数 是 功能 包 的 名 称 ;第 三 个 参数 是 该 软件 包 中 的 可 执行 
文件 的 名 称 。rosrun 本 质 上 是 通过 ROS 的 文件 结构 ， 找 到 对 应 包 的 可 执行 文件 并 执行 ， 与 直接 
指定 路 径 执行 可 执行 文件 没有 什么 特殊 的 区 别 。 

查看 节点 列表 。ROS 提供 了 一 些 方法 来 获取 任意 时 间 运 行 节点 的 信息 。 要 获得 运行 节点 列 
表 ， 使 用 如 下 命令 : 


rosnode list 


可 以 得 到 当前 正在 运行 的 节点 列表 。 例 如 ， 在 Roban 机 器 人 启动 后 打开 一 个 终端 运行 该 指 
令 即 可 得 到 如 下 列表 ， 得 知 机 器 人 中 当前 正在 运行 的 程序 。 


/ActExecPackageNode 
/BodyHubNode 
/SensorHubNode 
/camera/realsense2_camera 
/camera/realsense2_camera_manager 
/joystick_handle_node 
/ros_broadcast_node 
/ros_color_node 
/ros_gesture_node 
/ros_mic_arrays 
/ros_msg_node 
/ros_socket_node 
/ros_speech_node 
/ros_vision_node 

/rosout 

/usb_cam 


/web_video_server 


查看 节点 : 要 获得 特定 节点 的 信息 ， 使 用 如 下 命令 : 


A 第 3 章 ROS 使 用 概述 I> 49 


rosnode info node-name 


这 个 命令 的 输出 包括 话题 列表 一 一 节点 是 这 些 话题 的 发 布 者 〈publisher) 或 订阅 者 (sub- 
scriber)， 关 于 话题 请 参考 2.7.2 节 ; 服务 列表 一 这 些 服务 是 该 节点 提供 的 ， 关 于 服务 请 参考 第 
83€; 其 Linux 进程 标识 符 (process identifier, PID); 以 及 和 与 其 他 节点 的 所 有 连接 。 

2. 终止 节点 

要 终止 节点 ， 使 用 如 下 命令 : 


rosnode kill node-name 


通常 ， 也 可 以 使 用 Ctrl+C 组 合 键 来 终止 一 个 节点 的 运行 ， 但 使 用 这 种 方法 时 可 能 不 会 在 节 
点 管理 器 中 注销 该 节点 ， 因 此 会 导致 已 终止 的 节点 仍然 在 rosnode 列表 中 。 


3.3 ”话题 与 服务 


在 Roban 机 器 人 的 使 用 过 程 中 ， 各 个 节点 之 间 需 要 以 某 种 方式 进行 通信 ， 而 ROS 的 最 重要 
和 基础 的 特性 就 是 提供 了 这 种 机 制 。 这 种 通信 方式 可 以 极 大 地 方便 ROS 中 不 同 进程 和 可 执行 程 
序 之 间 的 通信 。 

ROS 节点 之 间 进 行 通信 所 利用 的 最 重要 的 机 制 就 是 消息 传递 。 在 ROS 中 ， 消 息 有 组 织 地 存 
放 在 话题 里 。 消 息 传递 的 理念 是 : 当 一 个 节点 想 要 分 享 信息 时 ， 它 就 会 发 布 (publish) 消息 到 对 
应 的 一 个 或 者 多 个 话题 ; 当 一 个 节点 想 要 接收 信息 时 , 它 就 会 订阅 (subscribe) 所 需要 的 一 个 或 
者 多 个 话题 。ROS 节点 管理 器 负责 确保 发 布 节点 和 订阅 节点 能 找到 对 方 ， 而 且 消 息 是 直接 地 从 
发 布 节点 传递 到 订阅 节点 ， 中 间 并 不 经 过 节点 管理 器 转交 。 

由 于 消息 机 制 是 一 种 单 向 传输 的 机 制 ， 在 消息 发 出 后 不 存在 响应 ， 甚 至 也 不 能 保证 系统 中 
有 其 他 节点 订阅 了 这 个 消息 ， 而 且 同 一 个 话题 〈topic) 可 能 同时 出 现 许 多 发 布 者 和 订阅 者 的 情 
况 ， 会 搞 不 清 数 据 来 源 。 因 此 ， 为 了 解决 这 个 问题 ，ROS 还 会 提供 另外 一 个 称 为 服务 〈service ) 
的 通信 机 制 用 于 通信 。 服 务 机 制 和 通常 互联 网 的 服务 机 制 很 相似 ， 即 一 个 节点 可 以 向 另 一 个 节 
点 发 送 请 求 并 且 等 待 响应 。 


3.3.1 ROS 话题 


要 查看 节点 之 间 的 连接 关系 ， 将 其 表示 为 图 形 是 最 便于 查看 的 。 在 ROS 中 查看 节点 之 间 的 
发 布 -订阅 关系 ， 最 简单 的 方式 就 是 在 终端 输入 如 下 命令 ; 


rqt_graph 
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在 这 个 命令 中 , r 代表 ROS; qt 指 的 是 用 来 实现 这 个 可 视 化 程序 的 Qt 图 形 用 户 界 面 《GUI) T. 
有 具 包 。 输 入 该 命令 之 后 ， 将 会 显示 一 个 图 形 界面 ， 其 中 大 部 分 区 域 用 于 展示 当前 系统 中 的 节点 。 
通常 情况 下 , 将 会 显示 一 个 表达 节点 之 间 连 接 关系 的 关系 图 , 在 该 图 中 , 椭圆 形 表示 节点 ， 有 向 
边 表示 其 两 端 节 点 间 的 发 布 -- 订 阅 关系。 可 能 会 注意 到 ， 我 们 发 现 的 rosout 节点 并 不 在 此 图 中 。 
这 是 因为 ， 在 默认 情况 下 ，rqt_graph 隐藏 了 其 认为 只 在 调试 过 程 中 使 用 的 节点 。 可 以 通过 取消 
“Hide debug ”选项 来 禁止 这 个 特性 。 

rqt_graph 还 有 其 他 一 些 选项 用 于 微调 显示 的 计算 图 。 通常 , 可 以 将 下 拉 选 项 中 的 Nodes only 
改 为 Nodes/Topics(all)， 并 取消 除 “Hide debug” 以 外 的 所 有 复 选 框 。 这 种 设置 的 好 处 在 于 能 用 
矩形 框 显 示 所 有 rqt_graph 工具 ， 尤 其 是 按照 上 述 进 行 设置 ， 能 帮助 发 现 自己 的 程序 可 以 用 哪些 
话题 来 和 现 有 节点 进行 通信 。 


3.3.2 ROS 消息 与 消息 类 型 


截至 目前 ， 我 们 已 经 了 解 了 这 些 节点 能 相互 传递 消息 ， 但 这 些 消息 里 到 底 包 含 了 什么 信息 ， 
我 们 对 此 还 是 一 无 所 知 。 下 面 ， 我 们 将 深入 探讨 话题 和 消息 。 

1. 话题 列表 

为 了 获取 当前 活跃 的 话题 ， 使 用 如 下 命令 : 


rostopic list 


在 Roban 机 嚣 人 中 将 列 出 很 多 话题 ， 这 里 摘录 如 下 : 


/ActRunner/DeviceList 


/BodyHub/SensorControl 

/Finish 

/LFootZ 
/MediumSize/ActPackageExec/Status 
/MediumSize/BodyHub/HeadPosition 
/MediumSize/BodyHub/MotoPosition 
/MediumSize/BodyHub/SensorRaw 
/MediumSize/BodyHub/ServoPositions 
/MediumSize/BodyHub/Status 
/MediumSize/BodyHub/WalkingStatus 
/MediumSize/SensorHub/BatteryState 
/MediumSize/SensorHub/Humidity 
/MediumSize/SensorHub/Illuminance 


/MediumSize/SensorHub/Imu 
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/MediumSize/SensorHub/MagneticField 
/MediumSize/SensorHub/Range 
/MediumSize/SensorHub/Temperature 
/MediumSize/SensorHub/sensor_CF1 
/RFootZ 

/VirtualPanel 
/camera/color/camera_info 


/camera/color/image_raw 


2. 打印 消息 内 容 
为 了 碍 看 茶 个 话题 上 发 布 的 消息 ， 可 以 利用 rostopic 命令 ， 形 式 如 下 : 


rostopic echo topic-name 


3. 测量 发 布 频率 
有 两 个 命令 可 以 用 来 测量 消息 发 布 的 频率 ， 以 及 这 些 消 息 所 占用 的 带宽 : 


rostopic hz topic-name 


rostopic bw topic-name 


这 些 命令 用 于 订阅 指定 的 话题 ， 并 且 输 出 一 些 统计 量 。 其 中 ， 第 一 条 命令 输出 每 秒 发 布 的 消息 
数量 ， 第 三 条 命令 输出 每 秒 发 布 消息 所 占 的 字 节 量 。 

注意 ， 有 时 我 们 不 关心 这 个 特定 的 频率 ， 但 是 这 些 命令 对 调试 很 有 帮助 ， 因 为 它们 提供 了 
一 种 简单 的 方法 用 来 验证 这 些 消息 确实 有 规律 地 在 向 这 些 特定 的 话题 发 布 。 

4. 查看 话题 

利用 rostopic info 命令 ， 可 以 获取 更 多 关于 话题 的 信息 : 


rostopic info topic-name 


以 视觉 图 像 为 例 ， 利 用 rostopic info 命令 同样 可 以 获取 一 些 信 息 : 


rostopic info /camera/color/image_raw 


在 Roban 机 器 入 中 使 用 上 述 指令 查看 视觉 图 像 的 原始 消息 ， 可 以 得 到 类 似 下 面 的 输出 : 


Type: sensor_msgs/Image 


Publishers: 
* /camera/realsense2_camera_manager (http: //lemon-NUC8i3BEH:43755/) 
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Subscribers: 
* /ros_gesture_node (http: //lemon-NUC8i3BEH:40575/) 
* /ros vision node (http://lemon-NUC8i3BEH: 41331/) 


输出 中 最 重要 的 部 分 是 第 一 行 ， 给 出 了 该 话题 的 消息 类 型 。 因 此 ， 在 /camera/colorimage_raw i 
题 中 发 布 -- 订 阅 的 消息 类 型 是 sensor. msgs/Image. Type 之 后 的 文本 输出 表示 数据 类 型 。 理 解数 
据 类 型 很 重要 ， 因 为 它 决定 了 消息 的 内 容 。 也 就 是 说 ， 一 个 话题 的 消息 类 型 能 告诉 你 该 话题 中 
每 个 消息 携带 了 哪些 信息 ， 以 及 这 些 消 息 是 如 何 组 织 的。 而 后 面 的 数据 代表 了 与 这 个 消息 的 发 
布 者 和 订阅 者 相关 的 信息 ， 比 如 可 以 发 现 这 个 消息 是 由 camera/realsense2_camera_manager 所 发 
布 出 来 的 ; 而 这 个 消息 分 别 被 ros_gesture_node 节点 和 ros_vision_node 节点 所 订阅 并 分 别 用 于 手 
势 识 别 的 处 理 和 视觉 图 像 识 别处 理 。 

5. 查看 消息 类 型 

要 想 查 看 某 种 消息 类 型 的 详情 ， 使 用 类 似 下 面 的 命令 : 


rosmsg show message-type-name 


对 上 面 用 到 的 /sensor_msgs/Image 消息 类 型 使 用 以 下 这 个 命令 ; 


rosmsg show sensor_msgs/Image 


其 结果 为 


std_msgs/Header header 


uint32 seq 

time stamp 

string frame_id 
uint32 height 
uint32 width 
string encoding 
uint8 is_bigendian 
uint32 step 
uint8[] data 


上 述 输出 的 格式 是 域 (feld) 的 列表 ， 每 行 一 个 元 素 。 每 个 域 由 基本 数据 类 型 “例如 int8、 
bool， 或 者 string) 以 及 域名 称 定 义 。 从 上 述 输出 中 可 以 看 出 消息 为 图 像 数据 类 型 。 注 意 ， 其 中 
包含 了 两 部 分 ， 一 部 分 为 包头 ， 用 于 发 布 图 像 的 序列 号 、id DRIER; 另外 一 部 分 为 实 
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际 的 凸显 内 容 ， 用 于 发 布 图 像 的 长 宽 、 编 码 形式 、 大 小 端 ， 以 及 图 像 中 每 个 像素 的 实际 数据 等 
信息 。 而 Header 部 分 就 是 一 个 典型 的 复合 域 ， 其 中 包含 了 不 同 的 消息 数据 信息 ， 值 得 注意 的 是 ， 
这 个 包头 在 不 同 的 消息 类 型 中 经 常会 出 现 。 

一 般 来 说 ， 一 个 复合 域 是 由 简单 的 一 个 或 者 多 个 子 域 组 合 而 成 ， 其 中 的 每 个 子 域 可 能 是 另 
一 个 复合 域 或 者 独立 的 域 ， 而 且 它们 一 般 也 都 由 基本 数据 类 型 组 成 。 同 样 的 思想 也 出 现在 C++ 
以 及 其 他 面向 对 象 的 编程 语言 中 ， 即 对 象 的 数据 成 员 可 能 是 其 他 对 象 。 

消息 类 型 同样 可 以 包含 固定 或 可 变 长 度 的 数组 〈 用 中 括号 口 表 示 ) 和 常量 (一 般 用 来 解析 
其 他 非常 量 的 域 )。 例 如 ， 在 图 像 类 型 中 有 uint8[] data 就 是 一 个 可 变 长 度 的 数组 类 型 。 


3.3.3 ROS 服务 
下 面 介绍 ROS 服务 的 基本 信息 流 。 服 务 调用 的 基本 原理 如 图 3.1 所 示 。 


request 
eee ee 
response - 


3.1 服务 调用 基本 原理 图 


其 过 程 是 一 个 客户 端 (client) 节点 发 送 一 些 称 为 请 求 (request) 的 数据 到 一 个 服务 器 (server) 
节点 ， 并 且 等 竺 回应。 服务 器 节点 接收 到 请 求 后 ， 采 取 一 些 行动 《计算 、 配 置 软件 或 硬件 、 改 变 
自身 行为 等 )， 然 后 发 送 一 些 称 为 响应 〈response) 的 数据 给 客户 端 节 点 。 

请 求 和 响应 数据 携带 的 特定 内 容 由 服务 数据 类 型 决定 ， 它 与 决定 消息 内 容 的 消息 类 型 是 类 
似 的 。 与 消息 类 型 一 样 ， 服 务 数据 类 型 也 是 由 一 系列 域 构成 的 。 唯 一 的 区 别 就 在 于 服务 数据 类 
型 分 为 两 部 分 ， 分 别 表示 请 求 〈 客 户 端 节 点 提供 给 服务 节点 ) 和 响应 (服务 节点 反馈 给 客户 端 
PAo 

尽管 服务 通常 由 节点 内 部 的 代码 调用 ， 但 是 也 确实 存在 一 些 命令 行 工具 来 与 之 交互 。 利 用 
这 些 工具 开展 实验 ， 能 够 帮助 我 们 更 容易 地 理解 服务 调用 的 工作 原理 。 

1. 列 出 所 有 服务 

通过 下 面 这 条 指令 ， 可 以 获取 目前 活跃 的 所 有 服务 : 


rosservice list 


在 Roban 机 器 人 上 运行 这 条 指令 ， 可 以 获取 目前 活路 的 所 有 服务 。 在 Roban 机 器 人 上 运行 
这 条 指令 获得 的 服务 列表 截取 部 分 如 下 : 


/ActExecPackageNode/get_loggers 


/ActExecPackageNode/set_logger_level 


/BodyHubNode/get_loggers 
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/BodyHubNode/set_logger_level 
/MediumSize/ActPackageExec/EndingExec 
/MediumSize/ActPackageExec/GetStatus 
/MediumSize/ActPackageExec/StateJump 
/MediumSize/ActPackageExec/actNameString 
/MediumSize/BodyHub/DeleteSensor 
/MediumSize/BodyHub/DirectMethod/GetServoLockStateAll 
/MediumSize/BodyHub/DirectMethod/GetServoPositionAll 
/MediumSize/BodyHub/DirectMethod/GetServoPositionValAll 
/MediumSize/BodyHub/DirectMethod/InstRead 
/MediumSize/BodyHub/DirectMethod/InstReadVal 
/MediumSize/BodyHub/DirectMethod/InstWrite 
/MediumSize/BodyHub/DirectMethod/InstWriteVal 
/MediumSize/BodyHub/DirectMethod/SetServoLockState 
/MediumSize/BodyHub/DirectMethod/SetServoLockStateAll 
/MediumSize/BodyHub/DirectMethod/SetServoTarPosition 
/MediumSize/BodyHub/DirectMethod/SetServoTarPositionAll 
/MediumSize/BodyHub/DirectMethod/SetServoTarPositionVal | 
/MediumSize/BodyHub/DirectMethod/SetServoTarPositionValAll | 


/color. recognition 


可 以 看 到 ， 在 一 个 实际 机 器 人 的 运行 过 程 中 需要 用 到 很 多 各 种 各 样 的 服务 。 每 行 都 表示 一 
个 当前 可 以 调用 的 服务 名 。 服 务 名 是 计算 图 源 名 称 ， 与 其 他 资源 名 称 一 样 ， 可 以 划分 为 全 局 的 、 
相对 的 或 者 私有 的 名 称 。rosservice list 命令 的 输出 是 所 有 服务 的 全 局 名 称 。 

本 例 中 的 服务 以 及 很 多 ROS 服务 总 的 来 讲 可 以 划分 为 两 个 基本 类 型 ; 

一 些 服务 ， 例 如 服务 列表 中 get loggers 和 set logger level 服务 ， 是 用 来 从 特定 的 节点 获取 
或 者 向 其 传递 信息 的 。 这 类 服务 通常 将 节点 名 用 作 命 名 空间 来 防止 命名 冲突 ， 并 且 允 许 节点 通 
过 私有 名 称 来 提供 服务 ， 例 如 get_loggers 或 者 set_logger_level。 

其 他 服务 表示 更 一 般 的 不 针对 某 些 特定 节点 的 服务 。 例 如 ， 名 为 /color recognition 的 服务 用 
于 对 颜色 识别 进行 配置 ， 是 由 turtlesim 节点 提供 的 。 但 是 在 不 同 的 系统 中 ， 这 个 服务 完全 可 能 
由 其 他 节点 提供 ; 当 调 用 /color_recognition 时 ， 我 们 只 关心 对 颜色 识别 的 配置 ， 而 不 关心 具体 哪 
个 节点 在 起 作用 。 服 务 列 表 列 出 的 所 有 服务 ， 除 了 get_loggers 和 set_logger_level， 都 可 以 归 入 
此 类 。 这 类 服务 都 有 特定 的 名 称 来 描述 它们 的 功能 ， 却 不 会 涉及 任何 特定 节点 。 


第 3 章 ROS 使 用 概述 I> 55 


2. 查看 服务 类 型 和 服务 的 数据 类 型 
(1) 查看 某 个 节点 的 服务 类 型 。 要 查看 一 个 特定 节点 所 提供 的 服务 ， 使 用 rosnode info 命令 : 


rosnode info node-name 


使 用 这 条 命令 可 以 得 到 /ActExecPackageNode 节点 所 提供 的 相应 服务 如 下 : 


Publications: 
* /MediumSize/ActPackageExec/Status [std_msgs/UInt16] 
* /rosout [rosgraph_msgs/Log] 


Subscriptions: None 


Services: 

* /ActExecPackageNode/get_loggers 

|* /ActExecPackageNode/set logger level 

| /MediumSize/ActPackageExec/GetStatus 

* /MediumSize/ActPackageExec/StateJump 

* /MediumSize/ActPackageExec/actNameString 


可 以 看 到 ActExecPackageNode 功能 包 提供 了 状态 跳 转 所 需 的 相关 服务 以 及 按 动作 名 称 运行 
的 服务 。 

(2) 查看 服务 的 数据 类 型 。 当 服务 的 数据 类 型 已 知 时 ， 可 以 使 用 rossrv 指令 来 获得 此 服务 
数据 类 型 的 详情 : 


rossrv Show service-data-type-name ‘ | 


例如 : 


rossrv show actexecpackage/SrvActScript | 


可 以 得 到 : 


string actNameReq 


string actResultRes 


在 这 里 ， 短 横 线 “---” 之 前 的 数据 是 请 求 项 ， 这 是 客户 节点 发 送 到 服务 节点 的 信息 。 短 横 
线 之 后 的 所 有 字段 是 响应 项 ， 或 者 说 是 服务 节点 完成 请 求 后 发 送 回 请 求 节点 的 信息 ， 通 过 以 上 
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信息 可 以 得 知 在 这 里 的 请 求 本 质 上 是 发 送 一 个 机 器 人 动作 文件 的 路 径 ， 然 后 使 得 机 器 人 可 以 自 
行 运行 这 个 路 径 。 

有 一 点 要 引起 注意 ， 服 务 数据 类 型 中 的 请 求 或 响应 字段 可 以 为 空 ， 甚 至 两 个 字段 可 以 同时 
为 室 ， 即 请 求 和 响应 字段 均 为 空 。 这 一 点 大 致 和 C++ 中 的 函数 可 以 接收 空 的 参数 并 返回 void 类 
似 。 虽 然 没 有 信息 进出 ， 但 仍然 可 能 有 用 《比如 该 服务 请 求 只 是 代表 一 个 特定 状态 的 变更 )。 


3.4 launch 文件 与 参数 


ROS 提供 了 一 个 同时 启动 节点 管理 器 Cmaster) 和 多 个 节点 的 途径 ,即使 用 launch 文件 ( 启 
动 文件 )。 事 实 上 ， 在 ROS 功能 包 中 7 启动 文件 的 使 用 是 非常 普遍 的 。 任 何 包含 两 个 或 两 个 以 
上 节点 的 系统 都 可 以 利用 启动 文件 来 指定 和 配置 需要 使 用 的 节点 。 本 章 将 对 启动 文件 和 运行 启 
动 文件 的 roslaunch 工具 进行 介绍 。 


3.4.1 launch 文件 介绍 


首先 ， 我 们 来 看 roslaunch 的 定义 。 其 基本 思想 是 在 一 个 XML 格式 的 文件 内 将 需要 启动 的 
节点 和 对 应 的 参数 罗列 出 来 。 下 面 以 Roban 机 器 人 中 的 一 个 launch 文件 为 例 ， 对 于 launch 文件 
中 的 元 素 进行 讲解 。 


<launch> 
<group> 
<arg name = "sim" default = "false" /> 
<param name = "poseOffsetPath" value = "(findbodyhub)/config/offset.yaml" /> 
<paramname = "poseInitPath" value = "(find bodyhub)/config/dxlInitPose.yaml"/» 
<param name = "SimulationInitPosePath" value = "(findbodyhub) /config/ 
SimulationInitPose.yaml" /> . 
<paramname = "sensorNameIDPath" value = "(find bodyhub)/config/sensorNameID. 
yaml" /> 
<param unless="(argsim)" name="simenable" value="false" /> 
<paramif="(arg sim)" name="simenable" value="false" /> 
<!-- BodyHubNode --> 
<node pkg="bodyhub" type="BodyHubNode" name="BodyHubNode" output="screen" /> 
</group> 
</launch> 


执行 launch 文件 : 
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想 要 运行 一 个 launch 文件 ， 可 以 像 下 面 这 样 使 用 roslaunch 命令 : 


roslaunch package-name launch-file-name 


例如 ， 如 果 要 执行 上 述 的 launch 文件 ， 可 以 执行 以 下 命令 : 


roslaunch bodyhub bodyhub. launch 


该 指令 正常 运行 之 后 ， 机 器 人 会 读 取 当 前 各 关节 位 置 ， 并 加 锁 ， 然 后 绥 缓 恢复 到 站 立 状 态 。 

与 其 他 ROS 文件 一 样 ， 每 个 launch 文件 都 应 该 和 一 个 特定 的 功能 包 关联 起 来 。 通 常 的 命 
名 方案 是 以 .launch 作为 launch 文件 的 扩展 名 。 最 简单 的 方法 是 把 launch 文件 直接 存储 在 功能 
包 的 根 目 录 中 。 当 查找 launch 文件 时 ，roslaunch 工具 会 同时 搜索 每 个 功能 包 目 录 的 子 目 录 。 包 
括 ROS 核心 包 在 内 的 很 多 功能 包 都 是 利用 这 一 特性 , 将 所 有 launch 文件 统一 存放 在 一 个 子 目录 
H, 该 子 目 录 通 常 取 名 为 launch。 比如 上 述 作为 例子 的 Bodyhub.launch 就 放 在 /bodyhub/launch 路 
fF 

最 简单 的 launch 文件 由 一 个 包含 若干 节点 元 素 (node elements) 的 根 元 素 (root element) 组 
成 。 揪 入 根 元 素 launch 文件 是 XML 格式 文件 ， 每 个 XML 格式 文件 都 必须 要 包含 一 个 根 元 素 。 
对 于 ROS launch 文件 ， 根 元 素 由 一 对 launch 标签 定义 ; 


<launch> 


/<launch> 


任何 launch 文件 的 核心 都 是 一 系列 的 节点 元 素 ， 每 个 节点 元 素 指 向 一 个 需要 启动 的 节点 。 
节点 元 素 的 形式 为 


pkg="package-name" 
type="executable-name" 


name-"node-name" 


每 个 节点 元 素 有 如 下 3 个 必需 的 属性 : 
pkg 和 type 属性 定义 了 ROS 应 该 运行 哪个 程序 来 启动 这 个 节点 。 这 些 和 rosrun 的 两 个 命令 
行 参数 的 作用 是 一 致 的 ， 即 给 出 功能 包 名 和 可 执行 文件 的 名 称 。name 属性 给 节点 指派 了 名 称 ， 
它 将 覆盖 任何 通过 调用 ros::int 来 赋予 节点 的 名 称 ， 通 过 这 种 方式 的 使 用 使 得 节点 副本 运行 。 
查看 节点 日 志文 件 使 用 roslaunch 启动 一 组 节点 与 使 用 rosrun 单独 启动 每 个 节点 的 一 个 重要 
不 同 点 是 ， ERARE P. 从 launch 文件 启动 节点 ee ee 志文 件 中 ， 而 
不 是 在 控制 台 显 示 。 该 日 志文 件 的 名 称 是 : 
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7/.ros/log/run, id/node, name-number-stout.log 


其 中 ，run_id 是 节点 管理 器 启动 时 生成 的 一 个 唯一 标示 符 。 
在 控制 台中 输出 信息 对 于 某 个 单独 的 节点 ， 只 需 在 节点 元 素 中 配置 output 属性 就 可 以 达到 
该 目的 : 


output="screen" 


配置 了 该 属性 的 节点 会 将 标准 输出 显示 在 屏幕 上 而 不 是 记录 到 之 前 讨论 的 日 志文 档 ， 在 以 
上 例子 中 的 bodyhub 节点 就 是 这 样 示例 程序 对 subpose 节点 配置 了 该 属性 , 这 就 是 为 什么 该 节 
点 的 INFO 信息 会 出 现在 控制 人 台中， 同时 也 说 明了 为 什么 该 节点 没有 出 现在 之 前 提 到 的 日 志文 
件 列表 中 。 除 了 影响 单个 节点 输出 信息 的 output 属性 之 外 ,还 可 以 使 用 screen 命令 行 选项 来 令 
roslaunch 在 控制 台中 显示 所 有 节点 的 输出 : 


roslaunch _screen package-name launch-file-name 


如 果 想 在 launch 文件 中 包含 其 他 launch 文件 的 内 容 〈 包 括 所 有 的 节点 和 参数 )， 可 以 使 用 
包含 (include) 元 素 : «include file="path-to-launch-file">， 此 处 file 属性 的 值 应 该 是 期 望 包含 文 
件 的 完整 路 径 。 由 于 直接 输入 路 径 信息 烦琐 且 容 易 出 错 ， 大 多 数 包含 元 素 都 使 用 查找 (find) ar 
令 搜索 功能 包 的 位 置 来 蔡 代 直接 输入 路 径 : 


| 


<include file="$(find package-name)/launch-file-name"> | 


如 果 给 定 的 功能 包 存在 , 则 上 面 小 括号 中 的 find 及 其 参数 将 展开 为 这 个 功能 包 的 路 径 , 比 在 
这 个 路 径 下 找到 想 要 的 lunch 文件 要 容易 得 多 。 例 如 ， 上 面 所 分 析 的 例子 中 就 包含 了 
$(find bodyhub)， 该 例子 中 使 用 这 个 路 径 指 向 了 bodyhub 功能 包 的 路 径 。 

为 了 声明 一 个 参数 ， 可 以 使 用 arg 元 素 。 


arg name="agr-name"> | 


对 于 参数 的 声明 不 是 必需 的 ， 但 是 这 样 能 使 launch 文件 的 使 用 者 更 加 清楚 launch 文件 需要 
哪些 参数 。 赋 值 有 很 多 种 方法 ， 例 如 可 以 像 下 面 在 roslaunch 命令 行 中 提供 该 值 : 


roslaunch package-name launch-file-name arg-name:=arg-value | 


除 此 之 外 ， 也 可 以 使 用 以 下 两 种 语法 ， 将 参数 值 作为 arg 声明 的 一 部 分 : 
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<arg name="arg-name" default="arg-value"/> 


<arg name="arg-name" value="arg-value"/> 


两 者 的 唯一 区 别 在 于 命令 行 参数 可 以 default， 但 是 不 能 覆盖 value。 获 取 的 参数 值 一 旦 被 声明 并 
且 被 赋值 ， 就 可 以 利用 下 面 的 arg 替换 Carg substitution) 语法 来 使 用 该 参数 值 了 ; 


$(arg arg-name) | 


在 每 个 该 替换 出 现 的 地 方 ，roslaunch 都 将 它 蔡 换 成 参数 值 。 

对 于 我 们 分 析 的 launch 文件 的 实例 中 ， 例 如 poseOffsetPath 就 是 指定 了 机 器 人 零点 文件 的 
路 径 ， 另 外 还 要 一 个 运行 launch 时 指定 的 参数 $(arg sim)， 用 于 指定 当前 node 是 用 于 仿真 的 情 
况 还 是 实物 使 用 的 情况 。 


3.4.2 ”机 器 人 实践 


通过 launch 配置 机 器 人 初始 动作 文件 。 
接 下 来 我 们 举 一 个 例子 来 说 明 launch 文件 参数 的 作用 , 例如 在 项 目的 cong 文件 夹 中 有 一 个 
dxlInitPose.yaml 配置 文件 用 于 定义 机 器 人 的 启动 动作 ，launch 文件 中 和 其 相关 的 内 容 如 下 : 


<param name = "poseInitPath" value = "$(find bodyhub)/config/dxlInitPose.yaml" /> 


这 个 文件 的 内 容 是 Roban 机 器 人 启动 动作 的 各 关节 的 角度 。 


#ANGLE -180 ~ +180 
InitPose: 
IDi: 0 
ID2: 1.46268 
ID3: 16.4558 
ID4: -34.0578 
ID5: -17.6019 
ID6: -1.46268 
ID7: O 
ID8: 1.46268 
ID9: -16.4558 
ID10: 34.0578 
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IDii: 17.6019 
ID12: 1.46268 
ID13: 0 

IDi4: -50 
ID15: -30 
IDi6: 0 

ID17: 50 
ID18: 30 
ID19: 0 
ID20: 0 
ID21: 0 
ID22: 0 


假如 我 们 在 启动 时 希望 机 器 人 手臂 部 分 呈现 一 个 不 同 的 启动 动作 ， 那 么 就 需要 新 增 一 个 这 
种 文件 。 例 如 ， 将 ID 为 17 和 14 WIENE WAERDE 40"， 那 么 需要 对 应 地 修改 配置 文件 
WF: 


#ANGLE -180 ~ +180 


InitPose: 

IDi: 0 

ID2: 1.46268 
ID3: 16.4558 
ID4: -34.0578 
IDS: -17.6019 
ID6: -1.46268 
ID7: 0 

ID8: 1.46268 
ID9: -16.4558 
ID10: 34.0578 
ID11: 17.6019 
ID12: 1.46268 
ID13: 0 

ID14: -40 
ID15: -30 
ID16: 0 

ID17: 40 
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ID18: 30 
ID19: 
ID20: 
ID21: 
ID22: 


o oo o 


将 这 个 修改 过 的 文件 保存 到 cong 文件 夹 中 ， 并 将 其 命名 为 dxlInitPoseNew.yaml。 这 样 就 可 
以 通过 修改 配置 文件 路 径 的 形式 修改 指定 新 的 配置 文件 路 径 。 通 过 这 样 的 修改 ， 可 以 使 得 机 器 
人 在 启动 时 特定 的 两 个 关节 的 弯曲 角度 变 为 新 指定 配置 文件 中 的 角度 。 当 然 ， 在 需要 的 情况 下 
也 可 以 放 多 个 启动 动作 的 配置 文件 ,在 不 同 的 情况 下 使 用 launch 文件 指定 不 同 的 启动 动作 文件 。 
<param name = "poseInitPath" value = "$(find bodyhub)/config/dxlInitPoseNew.yaml" 

/> 
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TE ROS 的 使 用 过 程 中 ， 常 常 需要 对 机 器 人 的 程序 进行 三 些 调试 ， 也 需要 对 传感器 的 数据 进 
行 一 些 监控 ， 因 此 需要 使 用 一 些 可 视 化 的 工具 对 数据 进行 监控 。rqt 是 一 个 基于 qt 开发 的 ROS 
可 视 化 工具 ， 拥 有 扩展 性 好 、 灵 活 易 用 、 路 平台 等 特点 ， 主 要 作用 与 RViz 一 致 ， 都 是 可 视 化 的 ， 
但 是 与 RViz 相 比 ，rqt 的 每 个 工具 都 具有 比较 专 一 的 功能 ， 但 是 使 用 效果 较 好 。 


3.5.1 rqt plot 


直接 在 终端 调用 rqt. plot 语句 ， 即 可 实现 将 一 些 参数 尤其 是 动态 参数 以 曲线 的 形式 绘制 出 
来 。 当 我 们 在 开发 时 查看 机 器 人 的 原始 数据 ， 就 能 利用 rqt plot 将 这 些 原始 数据 用 曲线 绘制 出 
来 ， 而 且 非 常 直 观 ， 利 于 我 们 分 析 数 据 。 下 面 以 Roban 机 器 人 的 步 态 行走 过 程 为 例 说 明 rqt_plot 
的 使 用 。 如 图 3.2 所 示 ， 单 击 界面 中 的 加 号 可 以 添加 所 需要 监听 的 Topic， 此 处 选取 了 真实 机 器 
人 质心 位 置 的 控制 值 和 实际 值 。 通 过 观察 可 以 发 现 ， 在 双 足 支撑 期 间 质心 的 跟踪 效果 较 好 ， 但 
是 在 单 足 支撑 期 间 跟 踪 效 果 较 差 ， 这 也 是 符合 实际 的 物理 规律 的 。 


3.52 rqt img View 
rqt img View 是 一 个 专用 于 视觉 传感器 图 像 调试 的 调试 工具 ， 通 过 选择 需要 订阅 的 话题 来 
选取 需要 观察 的 话题 。 如 图 3.3 所 示 ， 可 以 看 到 当前 视觉 传感器 所 采集 到 的 原始 图 像 ， 也 可 以 看 


到 视觉 传感器 处 理 过 后 的 图 像 ， 方 便 对 机 器 人 进行 调试 ， 在 输入 窗 的 左上 角 可 以 看 到 视频 采集 
到 的 topic， 可 以 通过 更 改 topic 的 名 称 来 得 到 当前 使 用 的 topic 的 图 像 。 
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3.3 rqt img View 界面 


3.5.3 rqt graph 


rqt graph 用 来 显示 通信 和 架构， 也 就 是 之 前 提 到 的 节点 、 主 题 等 ， 例 如 当前 有 哪些 Node 和 
topic 在 运行 ， 消 息 的 流向 是 怎样 的 ， 都 能 通过 这 个 语句 显示 出 来 。 此 命令 由 于 能 显示 系统 的 全 
貌 ， 所 以 非常 实用 ,可 以 方便 地 分 析 各 节点 之 间 的 关系 。 从 图 3.4 中 可 以 看 出 , 手柄 的 处 理 节 点 
会 将 数据 发 送 给 步 态 节点 用 于 执行 步 态 数据 ， 也 有 节点 会 使 用 从 网 络 得 到 的 数据 ， 即 将 数据 转 
发 给 对 应 的 舵 机 驱动 部 分 ， 而 视频 流 节点 会 将 数据 发 送 给 脸 部 识别 节点 和 视觉 识别 节点 ， 用 于 
脸 部 识别 和 手势 识别 。 
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3.4 rqt graph 界面 
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3.6.1 ROS 编译 环境 搭建 与 测试 


首先 打开 http://wiki.ros.org/， 如 图 3.5 所 示 。 
选择 install 进入 ， 然 后 选择 ROS Kinetic Kame 这 个 版 本 进行 下 载 ， 如 图 3.6 所 示 。 
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随后 选择 系统 平台 ， 这 里 我 们 选择 Ubuntu， 如 图 3.7 所 示 。 


然后 进行 安装 ， 
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有 具体 步骤 如 下 : 
(1) 配置 Ubuntu 存储 库 以 允许 “iestricted”“universe”“multiverse” 如 图 3.9 所 示 。 


Downloadable from the Internet 
© @ Canonicat-supported free and open-source software (main) 
|B Commanitymaintained free and open-source software (universe) 
© Proprietary drivers for devices (restricted) 
E Software restricted by copyright or legal issues (multiverse) 
© source code. 
Download trom | htrpiy/ubuntumirarsukž neyubuoma 00000 ^ 
Installable from CD-ROM/DVD 


To instail from CD-ROM ar OVO, insert the medium into the drive 


n 1 = Bess 


图 3.9 配置 Ubuntu 存储 库 


(2) 设置 sources.list， 使 得 计算 机 接受 来 自 package.ros.org 的 软件 。 


sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb release -sc) main" > / | 


etc/apt/sources.list.d/ros-latest.list' 


(3) 设置 密 钥 。 


sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6 
BADE8868B17 2B4F42ED6FBAB17C654 


如 果 在 连接 keyserver 时 遇 到 问题 ， 可 以 尝试 在 前 面 的 命令 中 替换 hkp://pgp.mit.edu:80 或 
hkp://keyserver.ubuntu.com:80. 


如 果 在 代理 服务 器 后 面 ， 可 以 使 用 curl 代替 apt-key 命令 ， 这 将 很 有 帮助 : 


curl -sSL "http: //keyserver ubuntu. con/pks /lookup?op=getksearch-0xC1CF6ES1E6BADEBSSSB | 
172B4F42ED6FBAB17C654' | sudo apt-key add - 


(4) 安装 。 
首先 ， 确 保 Debian 软件 包 索 引 是 最 新 的 : 


sudo apt-get update 


ROS 中 有 许多 不 同 的 库 和 工具 ， 官 方 提供 了 4 种 默认 配置 的 安装 ， 这 里 选择 Desktop-Full 
版 本 (这 个 版 本 的 库 和 工具 比较 全 ): 


sudo apt-get install ros-kinetic-desktop-full 
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(5) 环境 设置 。 
每 次 启动 新 的 shell 时 ， 如 果 将 ROS 环境 变量 自动 添加 到 bash 会 话 中 ， 将 很 方便 : 


echo "source /opt/ros/kinetic/setup.bash" >> ^/.bashrc 
source ^/.bashrc 


如 果 安 装 了 多 个 ROS 发 行 版 ， 则 ~/.bashrc 必须 仅 为 当前 使 用 的 版 本 提供 setup.bash。 
如 果 只 想 更 改 当前 shell 环境 ， 则 可 以 输入 以 下 内 容 而 不 是 上 面 的 内 容 ; 


source /opt/ros/kinetic/setup.bash 


如 果 使 用 zsh 而 不 是 bash， 则 需要 运行 以 下 命令 来 设置 shell 环境 : 


echo "source /opt/ros/kinetic/setup.zsh" >>“/.zshrc 


source ^/.zshrc 


(6) 构建 软件 包 的 依赖 关系 。 

到 目前 为 止 , 已 经 安装 了 运行 核心 ROS 软件 包 所 需 的 软件 。 为 了 创建 和 管理 自己 的 ROS 工 
VER, 需要 安装 各 种 工具 和 分 发 的 需求 。 例如 ，rosinstall 是 一 个 常用 的 命令 行 工具 , 使 您 可 以 使 
用 一 个 命令 轻松 下 载 ROS 的 许多 软件 包 。 

要 安装 此 工具 和 其 他 依赖 关系 以 构建 ROS 软件 包 ， 请 运行 : 


sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python- 
wstool build-essential 


i 


(7) 初始 化 rosdep。 
在 使 用 多 个 ROS 工具 之 前 ， 需 要 初始 化 rosdep. rosdep 使 您 能 够 轻松 地 为 要 编译 的 源 安装 系 
统 依赖 性 ， 并 且 是 运行 ROS 中 某 些 核心 组 件 所 必需 的 。 如 果 尚 未 安装 rosdep， 请 执行 以 下 操作 : 


sudo apt install python-rosdep 


使 用 以 下 命令 ， 可 以 初始 化 rosdep: 


sudo rosdep init 


rosdep update 


Lb 


(8) 验证 是 否 安装 成 功 。 
在 终端 输入 roscore， 出 现 如 图 3.10 所 示 的 信息 则 说 明 安 装 成 功 。 
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3.10 ”安装 成 功 验证 界面 


1. ROS 命令 
程序 代码 分 布 在 众多 的 ROS 软件 包 中 ， 当 使 用 命令 行 工 具 〈 如 1s 和 cd ) 来 浏览 时 会 非常 
烦琐 ， 因 此 ROS 提供 了 专门 的 命令 工具 来 简化 这 些 操作 。 
利用 rospack 获取 软件 包 的 有 关 信 息 。 
$ rospack find [ 包 名 称 ] 


roscd 允许 直接 切换 (cd) 工作 目录 到 某 个 软件 包 或 者 软件 包 集 中 。 
$ rosca [本 地 包 名 称 [/ 子 目录 ]] | 


Tab 自动 补 全 。 

当 要 输入 一 个 完整 的 软件 包 名 称 时 会 变 得 比较 烦琐 ， 可 用 TAB 补 全 的 功能 。 

2. 创建 ROS & 

Packages 软件 包 是 ROS 应 用 程序 代码 的 组 织 单元 ， 每 个 软件 包 都 可 以 包含 程序 库 、 可 执行 
文件 、 脚 本 或 者 其 他 手动 创建 的 东西 。 

首先 创建 一 个 工作 空间 : 


$ mkdir -p ^/catkin ws/src 
|$ cd ^/catkin ws/ | 
$ catkin make | 


然后 使 用 catkin, create. pkg 创建 一 个 名 为 “beginner_tutorials” 的 ROS 4, 依赖 于 std msgs. 
rospy 和 roscpp。 
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$ cd “/catkin_ws/src 


$ catkin_create_pkg beginner_tutorials std_msgs rospy roscpp 


这 将 创建 一 个 beginner_tutorials ROS 包 ， 其 中 包含 package.xml 和 CMakeLists.txt， 其 中 填 
充 了 你 给 catkin_create_pkg 的 信息 。 
catkin create pkg 要 求 给 它 一 个 package_name 以 及 可 选 的 软件 包 依赖 关系 的 列表 : 


$ catkin_create_pkg <package_name> [depend1] . [depend2] [depend3] 


然后 需要 在 工作 空间 中 编译 这 个 新 建 的 ROS 包 ， 并 将 工作 空间 添加 到 ROS 环境 中 。 


$ cd ~/catkin_ws 


$ catkin_make 


$ . ~/catkin_ws/devel/setup.bash 


3. 编写 ROS 节点 

一 个 节点 其 实 是 ROS 程序 包 中 的 一 个 可 执行 文件 。ROS 节点 可 以 使 用 ROS 客户 库 与 其 他 
节点 通信 。 节 点 可 以 发 布 或 接收 一 个 话题 ， 也 可 以 提供 或 使 用 某 种 服务 。 

节点 是 ROS 中 非常 重要 的 一 个 概念 ， 为 了 帮助 初学 者 理解 这 个 概念 ， 这 里 举 一 个 通俗 的 例 
T: 例如 ， 我 们 有 一 个 机 器 人 和 一 个 遥控 器 ， 那 么 这 个 机 器 人 和 遥控 器 开始 工作 后 ， 就 是 两 个 
节点 。 遥 控 器 起 到 了 下 达 指 令 的 作用 ; 机 器 人 负责 监听 遥控 器 下 达 的 指令 ， 完 成 相应 动作 。 从 
这 里 我 们 可 以 看 出 ， 节 点 是 一 个 能 执行 特定 工作 任务 的 工作 单元 ， 并 且 能 够 相互 通信 ， 从 而 实 
现 一 个 机 器 人 系统 整体 的 功能 。 这 里 ， 我 们 把 遥控 器 和 机 器 人 简单 定义 为 两 个 节点 ， 实 际 上 在 
机 器 人 中 根据 控制 器 、 传 感 器 、 执 行 机 构 等 不 同 组 成 模块 ， 还 可 以 将 其 进一步 细 分 为 更 多 的 节 
点 ， 这 个 是 根据 用 户 编写 的 程序 来 定义 的 。 

下 面 在 beginner tutorials 中 创建 一 个 节点 ， 输 出 “hello world”， 首 先 在 beginner_tutorials 下 
的 src 文件 夹 下 新 建 一 个 ros_tutorial.py 文件 ， 然 后 输入 下 面 的 代码 : 


#!/usr/bin/env Python 


import rospy 
rospy.init_node("ros_tutorial") 


rospy.loginfo("hello world") 


然后 将 ros_tutorial.py 设置 为 可 执行 的 : 


chmod +x ros_tutorial.py 


然后 在 工作 空间 下 执行 : 


69 


70h 仿 人 机 器 人 建 模 与 控制 


rosrun beginner_tutorials ros_tutorial.py 


将 会 输出 目 志 消息 “hello world”. 


代码 解析 : 
#!/usr/bin/env Python # 让 系统 知道 当前 是 可 执行 的 Python 脚本 
import rospy # 导入 rospy 包 ，rospy 是 R0S 的 Python 客户 端 


rospy.init_node("ros_tutorial") # 初始 化 一 个 名 为 “ros_tutorial” 的 ROS 节点 
rospy.loginfo("hello world") # 输出 日 志 信息 “hello world” 


3.6.2 ROS 话题 


ROS 节点 之 间 可 通过 messages 来 传递 消息 ， 话 题 是 用 于 识别 消息 的 名 称 ， 节 点 可 以 发 布 消 
息 到 话题 ， 也 可 以 订阅 话题 以 接收 消息 。 一 个 话题 可 能 对 应 许多 节点 作为 话题 发 布 者 和 话题 订 
阅 者 ， 当 然 ， 一 个 节点 可 以 发 布 和 订阅 许多 话题 。 一 个 节点 对 某 一 类 型 的 数据 感 兴趣 ， 它 只 需 
订阅 相关 话题 即 可 。 一 般 来 说 。 话 题 发 布 者 和 话题 订阅 者 不 知道 对 方 的 存在 。 发 布 者 将 信息 发 
布 在 一 个 全 局 的 工作 区 内 ， 当 订阅 者 发 现 该 信息 是 它 所 订阅 的 ， 就 可 以 接收 到 这 个 信息 。 

发 布 者 : 

生成 信息 , 通过 ROS 话题 与 其 他 节点 进行 通信 , 通常 用 于 处 理 原始 的 传感器 信息 ,如 相机 、 
编码 器 等 。 

订阅 者 : 

接收 信息 ， 通 过 ROS 话题 接收 来 自 其 他 节点 的 信息 ， 并 通过 回调 函数 处 理 ， 通 常用 于 监测 

1. 编写 一 个 简单 的 发 布 者 和 订阅 者 

首先 创建 一 个 发 布 者 节点 talker 该 节点 将 不 断 发 布 消 息 , 来 到 之 前 建立 的 beginner_tutorials 
Fae P: 


$ roscd beginner_tutorials 


然后 创建 一 个 scripts 目录 来 存放 我 们 的 Python 脚本 文件 : 


$ mkdir scripts 


$ cd scripts 


然后 在 scripts 目录 下 新 建 一 个 talker.py 文件 ， 创 建 我 们 的 发 布 者 ， 代 码 如 下 : 


#!/usr/bin/env Python 


import rospy 
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from std_msgs.msg import String 
def talker(): 
pub = rospy.Publisher('chatter', String, queue_size=10) 
rospy.init node('talker', anonymous=True) 
rate = rospy.Rate(10) # 10hz 
while not rospy.is_shutdown(): 
hello_str = "hello world %s" 7 rospy.get time() 
rospy.loginfo(hello. str) 
pub.publish(hello. str) 
rate.sleep() 


if |..name | == ' main, ': 
try: 
talker() 
except rospy.ROSInterruptException: 


pass 

代码 解析 : 
#!/usr/bin/env Python # 确保 您 的 脚本 作为 Python 脚本 执行 
import rospy # 导入 rospy 包 


from std_msgs.msg import String # 使 用 std_msgs/String 消息 类 型 


pub = rospy.Publisher('chatter', String) # 建立 一 个 发 布 者 ， 往 名 为 chatter 的 Topic 中 
# 发 布 String 类 型 的 消息 

rospy.init_node('talker', anonymous=True) # 初始 化 一 个 名 为 talker 的 节点 

r = rospy.Rate(10) # 设置 发 布 频率 为 10Hz 


while not rospy.is_shutdown(): 


hello str = "hello world 4s" % rospy.get_time() 
rospy.loginfo(hello str) 
pub. publish(hello_str) 
rate.sleep() 
# 此 循环 是 标准 的 rospy 结构 ; 检查 rospy.is_shutdown() 标 志 ， 然 后 进行 工作 。 必须 检查 
* is_shutdown() 以 检查 程序 是 否 应 该 退出 (例如 ， 是 否 存在 Ctr1+C 组 合 键 或 其 他 ) . pub.publish 
# () 使 用 新 创建 的 String 消息 发 布 到 我 们 的 chatter 话题 。 循环 调用 rate.sleep() 保 持 所 需 的 
# 速率 
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现在 ， 需 要 写 一 个 订阅 者 节点 去 接收 发 布 的 消息 。 同 样 在 scripts 目录 下 新 建 一 个 listener.py 
文件 ， 创 建 订阅 者 ， 代 码 如 下 : 


#!/usr/bin/env Python 


import rospy 
from std_msgs.msg import String 
def callback(data): 
rospy.loginfo(rospy.get caller id() + "I heard ^s", data.data) 


def listener(): 
rospy.init_node('listener', anonymous-True) 
rospy.Subscriber("chatter", String, callback) 
rospy.spin() 


if |, name | == '__main__': 


listener() 


代码 解析 : 


rospy.init node('listener', anonymous-True) # 初始 化 一 个 名 为 listener 的 节点 ， 
rospy.Subscriber("chatter", String, callback) # 声明 节点 订阅 类 型 为 std_msgs.msgs. 

# String 的 chatter 话题 。 收 到 新 消息 时 ， 将 以 消息 作为 第 一 个 参数 来 调用 回调 
rospy.spin() # 使 节点 无 法 退出 ， 直 到 该 节点 已 关闭 
# Anonymous = True 标志 告诉 rospy 为 该 节点 生成 一 个 唯一 的 名 称 ， 以 便 可 以 让 多 个 listener.py 节 
# 点 轻松 运行 


然后 来 到 工作 空间 目录 下， 编译 生 成 节点 ， 并 运行 代码 : 


$ cd ~/catkin_ws 


$ catkin_make 


运行 节点 前 需 设 置 为 可 执行 的 ， 同 时 需要 启动 roscore，roscore 是 节点 和 程序 的 集合 ， 是 基 
T ROS 的 先决 条 件 ， 必 须 运行 roscore， 才 能 使 ROS 节点 进行 通信 。 打 开 一 个 新 的 shell， 然 后 
输入 roscore。 图 3.11 所 示 为 roscore 运行 界面 。 

然后 运行 我 们 编写 的 talker 节点 和 listener 节点 ， 在 一 个 新 的 shell 中 输入 : 


$ rosrun beginner_tutorials talker.py 


将 会 看 到 发 布 者 打印 的 一 些 “hello world” 的 日 志 信息 ， 如 图 3.12 所 示 。 


第 3 章 “R0S 使 用 概述 i> 73 


roslaunch 


ted roslaunch server http: //lemon-NUCBL3BEH:38225/ 


13.11 roscore 运行 界面 


图 3.12 程序 运行 下 
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随后 ， 在 另 一 个 新 的 shell 中 输入 : 


$ rosrun beginner_tutorials listener.py 


将 会 看 到 订阅 者 收 到 来 自发 布 者 的 消息 ， 如 图 3.13 Pras. 


图 3.13 订阅 者 收 到 来 自发 布 者 的 消息 


2. 控制 turtlesim 运行 
首先 确保 roscore 已 经 运行 ， 打 开 一 个 新 的 终端 ， 


$ roscore 


如 果 没 有 退出 在 上 面 教 程 中 运行 的 roscore， 那 么 可 能 会 看 到 下 面 的 错误 信息 : 


roscore cannot run as another roscore/master is already running. 


Please kill other roscore/master processes before relaunching 


这 是 正常 的 ， 因 为 只 需要 有 一 个 roscore 在 运行 就 够 了 。 
在 本 书 中 ， 我 们 也 会 使 用 到 turtlesim， 请 在 一 个 新 的 终 St PEAT: 


$ rosrun turtlesim turtlesim_node 
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通过 键盘 远程 控制 turtle， 我 们 也 需要 通过 键盘 来 控制 turtle 的 运动 ， 请 在 一 个 新 的 终端 中 
运行 : 


$ rosrun turtlesim turtle_teleop_key 


[INFO] 1254264546.878445000: Started node [/teleop_turtle], pid [5528], bound on 

[aqy], xmlrpc port [43918], tcpros port [55936], logging to [~/ros/ros/log/teleop_ 
turtle 5528.1ogl, using [real] time 

Reading from keyboard 


Use arrow keys to move the turtle. 


现在 可 以 使 用 键盘 上 的 方向 键 来 控制 tartle 运动 了 。 如 果 不 能 控制 , 请 选中 turtle teleop key 
所 在 的 终端 窗口 以 确保 您 的 按键 输入 能 够 被 捕获 。 图 3.14 所 示 为 使 用 键盘 控制 目标 移动 。 
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现在 可 以 控制 turtle 运动 了 。turtlesim_node 节点 和 turtle_teleop_key 节点 之 间 是 通过 一 个 
ROS 话题 来 互相 通信 的 。turtle_teleop_key 在 一 个 话题 上 发 布 按键 输入 消息 ， 而 turtlesim 则 订阅 
该 话题 以 接收 该 消息 。 

使 用 rqt_graph: rqt_graph 能 够 创建 一 个 显示 当前 系统 运行 情况 的 动态 图 形 。rqt_graph 是 rqt 
程序 包 中 的 一 部 分 。 如 果 还 没有 安装 ， 请 通过 以 下 命令 来 安装 : 


$ sudo apt-get install ros-<distro>-rqt 


$ sudo apt-get install ros-<distro>-rqt-common-plugins 


请 使 用 你 的 ROS 版 本 名 称 ( 比 如 fuerte、groovy、hydro 等 ) 来 替换 掉 <distro>。 


在 一 个 新 终端 中 运行 


$ rosrun rqt_graph rqt_graph 
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就 会 看 到 类 似 图 3.15 所 示 的 图 形 。 
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如 果 将 鼠标 放 在 /turtlel/command_velocity 上 方 , 相应 的 ROS 节点 ( 蓝 色 和 绿色 ) 和 话题 ( 红 
色 ) 就 会 高 亮 显示 。 正 如 您 所 看 到 的 ，turtlesim_node 和 turtle_teleop_key 节点 正 通过 一 个 名 为 
/turtle1/command_velocity 的 话题 来 互相 通信 ， 如 图 3.16 所 示 。 
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图 3.16 ROS 节点 和 话题 


话题 之 间 的 通信 和 是 通过 在 节点 之 间 发 送 ROS 消息 实现 的 。 对 于 发 布 者 (turtle_teleop_key) 
和 订阅 者 Cturtulesim_node ) 之 间 的 通信 , 发 布 者 和 订阅 者 之 间 必 须发 送 和 接收 相同 类 型 的 消息 。 
这 意味 着 话题 的 类 型 是 由 发 布 在 它 上 面 的 消息 类 型 决定 的 。 

使 用 rostopic type 命令 可 以 查看 发 布 在 某 个 话题 上 的 消息 类 型 。 

FAYE: rostopic type [topic] 


$ rostopic type /turtlei/cmd, vel 


你 应 该 会 看 到 : 


geometry_msgs/Twist 


使 用 rosmsg 命令 可 以 查看 消息 的 详细 情况 : 


$ rosmsg show geometry_msgs/Twist 


geometry_msgs/Vector3 linear 
float64 x 
float64 y 
float64 z 


geometry_msgs/Vector3 angular 
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float64 x 
float64 y 
float64 z 


现在 我 们 已 经 知道 了 turtlesim 节点 所 期 望 的 消息 类 型 ， 接 下 来 就 可 以 给 turtle Rime T., 
见 图 3.17。 
使 用 rostopic pub 命令 可 以 把 数据 发 布 到 当前 某 个 正在 广播 的 话题 上 。 
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图 3.17 通过 消息 控制 目标 


用 法 : rostopic pub [topic] [msg_type] [args] 


$ rostopic pub -1 /turtle1/cmd_vel geometry msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 
0.0, 1.8]' 


| 
| 
L 


以 上 命令 会 发 送 一 条 消息 给 turtlesim， 告 诉 它 以 2.0 大 小 的 线 速 度 和 1.8 大 小 的 角速度 开始 
移动 。 
这 是 一 个 非常 复杂 的 例子 ， 因 此 让 我 们 来 详细 分 析 其 中 的 每 个 参数 。 


rostopic pub 这 条 命令 将 会 发 布 消息 到 某 个 给 定 的 话题 。 

-1 (BA FR) 这 个 参数 选项 使 rostopic 发 布 一 条 消息 后 马上 退出 。 

/turtlei/command velocity 这 是 消息 所 发 布 到 的 话题 名 称 。 

turtlesim/Velocity 这 是 所 发 布 消息 的 类 型 。 

-- RFR) 这 会 告诉 命令 选项 解析 器 接 下 来 的 参数 部 分 都 不 是 命令 选项 。 这 在 参数 里 面包 含有 
半 字 线 -( 比 如 负 号 ) 时 是 必须 要 添加 的 。 

2.0 1.8 正如 之 前 提 到 的 ， 在 一 个 turtlesim/Velocity 消 息 里 面包 含有 两 个 浮 点 型 元 素 : linear 和 
angular。 在 本 例 中 ，2.0 是 线 速度 的 值 ，1.8 是 角速度 的 值 


让 turtle 走 一 个 如 图 3.18 所 示 的 圆 形 的 轨迹 。 
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3.18 消息 控制 目标 


您 可 能 注意 到 turtle 已 经 停止 移动 了 。 这 是 因为 turtle 需要 一 个 稳定 的 频率 为 1Hz 的 命令 流 
来 保持 移动 状态 。 我 们 可 以 使 用 rostopic pub -r 命令 来 发 布 一 个 稳定 的 命令 流 。 


$ rostopic pub /turtlei/cmd, vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 
070, 17811 


这 条 命令 以 1Hz 的 频率 发 布 速度 命令 到 速度 话题 上 。 
我 们 会 看 到 turtle 会 一 直 走 一 个 圆 形 的 轨迹 。 


3.6.3 ROS 服务 


IRS EA RO IN TK, IRS SOR AE AOR IF. HRAS3R fi SCELUS 
是 一 对 一 的 同步 通信 方式 ， 服 务 包 含 Client Ej Server 两 部 分 ，Client 发 布 请 求 后 会 在 原 地 等 待 
reply， 直 到 服务 处 理 完了 请 求 并 且 完 成 了 reply, Client 才 会 继续 执行 。 

1. 编写 简单 的 service 和 client 

首先 建立 一 个 服务 消息 类 型 ， 用 于 相互 通信 ， 让 我 们 使 用 刚刚 创建 的 包 来 创建 srv: 


$ roscd beginner_tutorials 


$ mkdir srv 


我 们 从 另 一 个 软件 包 中 复制 一 个 现 有 的 srv 定义 。roscp 是 一 个 有 用 的 命令 行 工具 ， 用 于 将 
文件 从 一 个 软件 包 复 制 到 另 一 个 软件 包 。 


$ roscp [package_name] [file_to_copy_path] [copy_path] 


现在 我 们 可 以 从 rospy. tutorials 包 中 复制 服务 ; 


$ roscp rospy tutorials AddTwoInts.srv srv/AddTwoInts.srv 
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然后 打开 package.xml， 并 确保 其 中 两 行 都 没有 注释 : 


<build_depend>message_generation</build_depend> 


<exec_depend>message_runtime</exec_depend> 


然后 添加 message. generation 依赖 项 以 在 CMakeLists.txt 中 生成 消息 : 


find_package(catkin REQUIRED COMPONENTS 
roscpp 
rospy 
std_msgs 


message_generation 


此 外 ， 需 要 对 服务 的 package.xml 和 消息 进行 相同 的 更 改 : 


add_service_files( 
FILES 
AddTwoInts.srv 


现在 来 到 工作 空间 , 将 catkin_make 编译 一 下 ， 就 可 以 根据 服务 定义 生成 服务 消息 了 。 随 后 ， 
让 我 们 确认 一 下 ROS 可 以 使 用 rossrv show 命令 看 到 它 。 


$ rossrv show beginner_tutorials/AddTwoInts 


你 将 看 到 : 


int64 a 
int64 b 


int64 sum 


在 这 里 ， 我 们 将 创建 一 个 服务 节点 add_two_ints_server。 该 节点 将 接收 两 个 int 类 型 数据 并 
返回 它们 的 和 。 
将 目录 更 改 为 beginner_tutorials 包 : 


$ roscd beginner_tutorials 


在 scripts 目录 下 ， 创 建 一 个 add_two_ints_serverpy 文件 ， 输 入 以 下 内 容 : 


vlla! c6 


#!/usr/bin/env Python 
from beginner tutorials.srv import * 
import rospy 
def handle, add two, ints(req): 
print "Returning [hs + 4s = As]"A(req.a, req.b, (req.a + req.b)) 


return req.a * req.b 


def add two ints server(): 
rospy.init, node('add, two. ints, server') 
s = rospy.Service('add two ints', AddTwoInts, handle, add two. ints) 
print "Ready to add two ints." 
rospy.spin() 


if __name__ == " main  ": 


add two ints server() 


不 要 忘记 使 节点 可 执行 : 


chmod +x add_two_ints_server.py 


代码 解析 : 
我 们 使 用 init_node() 声明 节点 ， 然 后 声明 我 们 的 服务 ; 


s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints) 


这 将 声明 一 个 具有 AddTwolnts 服务 类 型 的 名 为 add_two_ints 的 服务 。 所 有 请 求 都 传递 给 
handle_add_two_ints 函数 。handle_add_two_ints 会 将 请 求 的 两 个 数 相 加 并 返回 。rospy.spin0 可 以 
防止 代码 退出 ， 直 到 服务 关闭 。 

现在 建立 一 个 client 节点 : 

来 到 beginner_tutorials &l, 在 scripts 目录 下 创建 一 个 add_two_ints_client.py 文件 , 输入 以 下 
内 容 : 


#!/usr/bin/env Python 


import sys 

import rospy 

from beginner_tutorials.srv import * 
def add_two_ints_client(x, y): 


rospy.wait_for_service('add_two_ints') 


eem P an 


try: 
add two ints = rospy.ServiceProxy('add two ints', AddTwoInts) 
respi - add two ints(x, y) 
return respi.sum 

except rospy.ServiceException, e: 


print "Service call failed: %s"%e 


def usage(): 
return "As [x y]"%sys.argv[0] 


if ..name, == " main ": 
if len(sys.argv) -- 3: 
x = int(sys.argv[1]) 
y 
else: 


int(sys.argv[2]) 


print usage() 
sys.exit(1) 
print "Requesting As*As"A(x, y) 


print "As + 4s = A4s"A(x, y, add two ints client(x, y)) 


AS BES 1 TET RAT: 


chmod +x add two ints client.py 


代码 解析 : 
对 于 client， 不 必 调 用 init_node()， 首 先 调用 : 


rospy.wait for service('add two ints') 


它 会 阻塞 直到 add. two. ints 服务 可 用 为 止 ， 接 下 来 ， 我 们 创建 一 个 用 于 调用 服务 的 句柄 ; 


add_two_ints = 


rospy.ServiceProxy('add two ints', AddTwoInts) 


我 们 可 以 像 普 通 函 数 一 样 使 用 此 句柄 并 调用 它 ; 


一 -一 -一 


respi = add_two_ints(x, y) 


return respi.sum 


上 面 将 int 类 型 的 两 个 数 x Aly 上 传 请 求 服务 ， 并 返回 它们 的 和 。 


niii eia 


编译 节点 。 
来 到 工作 室 间 ， 并 执行 catkin_make: 


$ cd ~/catkin_ws 


$ catkin_make 


运行 service， 打 开 一 个 新 的 终端 ， 执 行 : 


$ rosrun beginner_tutorials add_two_ints_server.py 


将 会 看 到 : 


Ready to add two ints. 


运行 client， 打 开 一 个 新 的 终端 ， 执 行 : 


$ rosrun beginner_tutorials add_two_ints_client.py 


将 会 看 到 类 似 如 下 的 使 用 消息 : 


/home/user/catkin_ws/src/beginner_tutorials/scripts/add_two_ints_client.py [x y] 


提示 我 们 输入 两 个 数 ， 重 新 输入 执行 : 


$ rosrun beginner_tutorials add_two_ints_client.py 4 5 


将 会 看 到 ， 


Requesting 4+5 
4+5=9 


同时 service 将 会 输出 : 


Returning [4 + 5 = 9] 


2. turtlesim 提供 的 服务 
首先 运行 roscore， 然 后 在 一 个 新 的 终端 中 运行 turtlesim_node: 


$ rosrun turtlesim turtlesim_node 


下 面 介绍 turtlesim 提供 的 服务 。 
rosservice 有 许多 可 用 于 服务 的 命令 ， 如 下 所 示 : 
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rosservice list # 显示 正在 运行 的 服务 的 信息 
rosservice call # 调用 服务 可 附加 参数 
rosservice type # 显示 服务 类 型 

rosservice find # 按 服 务 类 型 查找 服务 


rosservice list: 


$ rosservice list 


该 列表 命令 向 我 们 表明 ，turtlesim 节点 提供 以 下 服务 : 


/clear 
/kill 


/reset 


/rosout/get loggers 

/rosout/set logger level 

/spawn 

/teleop turtle/get loggers 
/teleop turtle/set logger level 
/turtlei/set. pen 
/turtlei/teleport absolute 
/turtlei/teleport relative 
/turtlesim/get loggers 


/turtlesim/set logger level 


其 中 有 两 个 与 单独 的 rosout 节点 相关 的 服务 : /rosout/get_loggers 和 /rosout/set_ logger. level. 


rosservice type: 


使 用 rosservice type 来 看 看 /clear 服务 : 


$ rosservice type /clear 
std_srvs/Empty 


a 


此 服务 为 室 ， 这 意味 着 在 进行 服务 调用 时 ， 它 不 接收 任何 参数 〈 即 ， 在 发 出 请 求 时 不 发 送 任何 


数据 ， 在 接收 响应 时 不 接收 任何 数据 )。 


现在 通过 键盘 来 控制 turtle 运动 ， 在 一 个 新 的 终端 中 运行 : 


$ rosrun turtlesim turtle_teleop_key 


程序 运行 结果 如 图 3.19 所 示 。 


H 
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rosservice call: 


使 用 rosservice call 调用 /clear 服务 : 


$ rosservice call /clear 


图 3.19 通过 键盘 来 控制 目标 运动 


它 清除 了 turtlesim_node 的 背景 ， 结 果 如 图 3.20 Aras. 


图 3.20 清除 节点 


接 下 来 查看 /spawn 服务 具有 参数 的 情况 : 


$ rosservice type /spawn | rossrv show 
float32 x 

float32 y 

float32 theta 


string name 
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| 


string name 


该 服务 使 我 们 可 以 在 给 定 的 位 置 和 方向 上 生成 新 的 turtle。 名 称 字段 是 可 选 的 ， 缺 省 时 
turtlesim 为 我 们 创建 一 个 turtle2， 依 次 递增 。 


$ rosserVice call /spawn 2 2 0.2 "" 


服务 调用 返回 新 创建 的 turtle 的 名 称 ， 并 且 会 在 窗口 中 生成 新 的 turtle, 结果 如 图 3.21 所 示 。 


|name: turtle2 


图 3.21 创建 新 目标 


3. 控制 Roban 头 部 运动 

控制 机 器 人 实质 是 控制 机 器 人 的 和 能 机。 在 Roban 机 器 人 中 有 一 个 动作 执行 的 状态 机 ， 状 态 
转换 结构 是 为 了 确保 当前 阶段 只 能 有 一 个 节点 对 这 个 动作 执行 节点 进行 占用 。 

首先 启动 Roban 机 器 人 ， 然 后 通过 服务 占用 动作 节点 : 


| rosservice call /MediumSize/BodyHub/StateJump 2 setStatus 


然后 通过 rostopic pub 设置 positions 为 [60,0] 让 头 部 舵 机 转向 60°: 


| rostopic pub /MediumSize/BodyHub/HeadPosition bodyhub/JointCorolPoint "positions: 
| [60,0] 

| Velocities: [0] 

| accelerations: [0] 

effort: [0] 


time_from_start: {secs: 0, nsecs: 0} 
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mainControlID: 2" 


随后 可 以 看 见 ，Roban 机 器 人 头 部 舵 机 向 右 转向 了 60°. 
37 主 从 机 配置 


ROS 主 从 机 配置 即 为 ROS 计算 机 分 布 式 主 从 通信 ， 是 需要 安装 ROS 环境 之 后 才能 配置 。 
ROS 主 从 机 通信 是 ROS 主机 和 从 机 通过 订阅 话题 和 服务 实现 的 ， 其 使 用 的 前 提 是 主机 的 ROS 
必须 启动 。 

ROS 的 主 从 环境 其 实 是 一 种 分 布 式 通信 方式 。 这 里 讲述 ROS 主 从 环境 的 配置 步骤 。 


3.7.1 “获取 IP 地 址 和 Hostname 


启动 机 器 人 后 ， 使 用 HDMI 线 、 键 盘 、 鼠 标 连 接 机 器 人 ， 并 设置 好 对 应 的 WiFi 后 ， 使 用 如 
下 命令 来 获取 Roban 和 计算 机 的 下 地 址 : 


$ ifconfig | grep "inet" 


结果 如 下 : 


inet addr:127.0.0.1 Mask:255.0.0.0 

inet6 addr: ::1/128 Scope:Host 

inet addr:192.168.2.18 Bcast:192.168.2.255 Mask:255.255.255.0 
inet6 addr: fe80::404f:d268:cf4a:d799/64 Scope:Link 


这 里 显示 我 们 有 两 个 耳 地 址 ， 即 127.0.0.1 和 192.168.2.18. 127.0.0.1 是 本 地 回路 的 下 地 址 ， 
而 192.168.2.18 是 我 们 当前 机 器 人 的 局 域 网 TP 地 址 。 
接 下 来 ， 我 们 使 用 下 面 的 指令 来 分 别 获取 机 器 人 和 计算 机 的 hostname: 


$ hostname 


lemon-NUC8i3BEH 为 Roban 机 器 人 的 hostname, 也 可 以 直接 查看 终端 标题 栏 “@” 后 面 的 内 
容 来 获得 当前 的 hostname。 WIH: 不 同 的 机 器 人 可 能 设置 存在 差异 , 请 按照 查询 得 到 的 hostname 
进行 设置 。 

WE, 已 获取 到 了 我 们 所 需要 的 4 个 参数 : 

1. 计算 机 的 下 地 址 : 192.168.2.3 

2. 计算 机 的 hostname: ak-pc 

3. Roban 的 IP 地 址 : 192.168.2.18 

4. Roban 的 hostname: — lemon-NUC8i3BEH 
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3.7.2 ”修改 对 应 的 hosts 


hosts 文件 位 于 ete 文件 夹 下 ， 修 改 hosts 文件 的 目的 是 将 两 台 计 算 机 的 IP 地 址 和 主机 名 绑 
XE, 使 两 台 计 算 机 通过 hostname 就 能 很 方便 地 找到 对 方 。 在 修改 host 文件 之 前 ， 直 接 ping 对 方 
主机 名 ， 是 无 法 解析 的 。 当 然 ， 即 便 不 配置 ， 也 可 以 直接 ping 对 方 的 下 地 址 。 但 是 为 了 后 期 的 
数据 识别 和 操作 ， 还 是 需要 将 耳 地 址 和 hostname 绑 定 起 来 。 在 我 们 的 计算 机 终端 中 输入 〈 输 入 
完成 后 可 能 需要 输入 密码 ， 请 注意 输入 的 密码 在 终端 并 不 会 显示 ): 


$ sudo gedit /etc/hosts 


显示 如 下 : 
127.0.0.1 localhost 
127.0.1.1 ak-pc 


# The following lines are desirable for IPv6 capable hosts 
$52 ip6-localhost ip6-loopback 

fe00::0 ip6-localnet 

ff00::0 ip6-mcastprefix 

ff02::1 ip6-allnodes 

ff02::2 ip6-allrouters 


可 以 看 到 ， 在 hosts 文件 内 的 开头 两 行 已 经 绑 定 了 两 个 IP 地 址 ， 插 入 刚刚 查 到 的 Roban 机 
器 人 的 IP 地址 和 hostname。 应 注意 ，IP 地 址 与 hostname 中 间 要 用 Tab 键 隔 开 ， 而 不 能 使 用 
空格 。 


192.168.2.18 lemon-NUC8i3BEH 


在 Roban 机 器 人 执行 同样 的 命令 ， 输 入 以 下 内 容 : 


192.168.2.3 ak-pc 


保存 修改 后 ， 退 出 hosts 文件 ， 使 用 如 下 命令 重启 网 络 : 


$ sudo /etc/init.d/networking restart 


MES 以 上 实际 输入 内 容 请 以 实际 记录 的 为 准 。 
3.7.3 ”配置 主 从 关系 


以 下 设置 中 ， 将 Roban 机 器 人 设置 为 主机 ， 计 算 机 端 设置 为 从 机 。 
在 双方 的 ~/,bashre 中 加 入 以 下 信息 : 
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export ROS IP-'hostname -I | awk '{print $1)" 
export ROS_HOSTNAME='hostname -I | awk '{print $1}" 
export ROS_MASTER_URI=http://lemon-NUC8i3BEH:11311 


对 于 Roban 机 器 人 而 言 ， 我 们 已 经 预 设 了 如 下 设置 : 


export ROS_IP='hostname -I | awk '{print $1)" | 
export ROS. HOSTNAME-'hostname -I | awk '{print $1)" 
export ROS. MASTER URI-http://localhost:11311 | 


修改 完成 后 ， 让 其 配置 生效 ， 使 用 如 下 命令 : 


source ^/.bashrc 


此 时 ， 主 从 关系 就 已 经 全 部 配置 完成 了 。Roban 机 器 人 在 启动 的 时 候 会 自动 打开 ROS 节点 ， 
所 以 此 时 只 要 在 计算 机 端 使 用 rosnode list 即 可 看 到 所 有 在 Roban 机 器 人 上 的 ROS 节点 , 说 明 已 
经 配置 成 功 。 


38 ROS CvBridge 实践 


ROS 以 其 自己 的 sensor_msgs/Image 消息 格式 传递 图 像 , 但 是 我 们 需要 使 用 OpenCV 来 处 理 
图 片 数 据 ， 此 时 就 需要 利用 CvBridge 提供 的 ROS 和 OpenCV 的 转换 接口 。 


3.8.1 ”将 ROS 图 像 消息 转换 为 OpenCy 的 图 像 


from cv_bridge import CvBridge | | 
bridge - CvBridge() 
cv image = bridge.imgmsg to. cv2(image message, desired encoding-'bgr8') | 


3.8.2 + OpenCV 图 像 转换 为 ROS 图 像 消息 


from cv_bridge import CvBridge 
bridge = CvBridge() 


image_message = bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") 
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3.83 ”在 计算 机 上 显示 Roban 机 器 人 摄像 头 数据 
运行 以 下 代码 已 经 默认 计算 机 中 做 好 了 主 从 机 配置 ， 且 安装 好 了 完整 版 的 ROS 环境 。 


#!/usr/bin/Python 


import rospy 


from cv_bridge import CvBridge 


import cv2 


from sensor_msgs.msg import Image 


def callback (image): 


cv_image = bridge.imgmsg_to_cv2(image, "bgr8") 
cv2.imshow("image", cv_image) 
cv2.waitKey(1) 


if | name | == "__main__": 


rospy.init_node('pc_image_view') 
camera_topic = "/camera/color/image_raw" 
bridge = CvBridge() 
rospy.Subscriber(camera_topic, Image, callback) 
rospy.spin() 


以 上 代码 位 于 : 


~/robot_ros_application/catkin_ws/src/ros_host_node/scripts/pc_images_view.py 


将 ros_host_node 这 个 package 复制 到 主机 的 工作 空间 中 ， 并 使 用 catkin_make 编译 程序 后 ， 
执行 如 下 命令 即 可 运行 : 


$ rosrun ros_host_node pc-images. view.py 


同步 定位 与 地 图 构建 


CHAPTER 4 


本 章 主要 讲述 和 SLAM 相关 的 内 容 。SLAM 是 一 个 可 移动 机 器 人 能 够 绘制 环境 的 地 图 并 且 
同时 使 用 该 地 图 进行 自我 定位 的 过 程 。 近 十 年 来 ， 随 着 SLAM 方法 的 许多 令 人 信服 的 应 用 ， 在 
处 理 SLAM 问题 方面 取得 了 迅速 且 令 人 兴奋 的 进步 。 本 章 开头 部 分 为 SLAM 简介 ， 然 后 主要 讲 
述 SLAM 的 基本 解决 方法 和 重要 的 实现 , 后 面 的 部 分 比较 详细 地 介绍 Roban 机 器 人 中 SLAM 的 
实现 过 程 ， 将 按照 接收 图 像 、 发 布点 云 数据 、 生 成 八 又 树 图 、 生 成 二 维 地 图 、 计 算 路 径 以 及 最 
终 实现 行走 的 顺序 逐一 介绍 。 


4.1 SLAM 简介 


同步 定位 与 地 图 构建 (Simultaneous Localization and Mapping, SLAM) 问题 可 以 描述 为 : 机 
器 人 在 未 知 环境 中 从 一 个 未 知 位 置 开 始 移动 ， 在 移动 过 程 中 根据 位 置 估计 和 地 图 进行 自身 定位 ， 
同时 在 自身 定位 的 基础 上 建造 增 量 式 地 图 ， 实 现 机 器 人 的 自主 定位 和 导航 。 一 个 SLAM 问题 的 
解决 方案 已 经 成 为 可 移动 机 器 人 的 “ 金 钥匙 ?>， 可 让 机 器 人 真正 实现 自主 行走 。 

为 了 四 处 走动 ， 机 器 人 需要 像 人 一 样 从 地 图 上 获得 信息 。 但 就 像 人 类 一 样 ， 机 器 人 也 不 能 
总 是 依靠 GPS, 尤其 是 在 室内 运行 时 。 并且 要 想 安 全 地 移动 需要 10cm 左右 的 安全 距离 ，GPS 也 
没有 足够 的 精度 来 支持 在 户外 的 运行 。 相 反 ， 如 果 机 器 人 可 以 依靠 同步 的 本 地 化 地 图 和 SLAM 
来 探测 与 绘制 周围 环境 ， 导 航 和 定位 便 将 精确 得 多 。 借 助 SLAM， 机 器 人 可 以 随时 随地 构建 自 
己 的 地 图 。 通 过 将 它们 收集 的 传感器 数据 与 它们 已 经 收集 的 任何 传感器 数据 对 齐 ， 以 建立 导航 
地 图 ， 让 它们 知道 自己 的 位 置 。 这 听 起 来 很 容易 ， 但 这 实际 上 是 一 个 多 阶段 的 过 程 ， 包 含 着 基 
于 强大 并 行 处 理 能 力 的 GPU 的 复杂 算法 的 处 理 器 数据 对 齐 。 

SLAM 最 早 由 Smith、Self 和 Cheeseman 于 1988 年 提出 ， 至 今 已 有 三 十 多 年 的 发 展 历史 。 
Hugh F. Durrant-Whyte 研究 小 组 在 20 世纪 90 年 代 初 期 进行 了 该 领域 的 其 他 开拓 性 工作 。 这 表 
明 SLAM 的 解决 方案 存在 于 无 限 数 据 限 制 中 。 该 发 现 激 励 了 对 在 计算 上 易 处 理 并 取 近 似 解 的 算 
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法 的 搜索 。 由 Sebastian Thrun 领导 的 自动 驾驶 STANLEY 和 JUNIOR 汽车 赢得 了 DARPA 大 挑 
战 赛 ， 并 在 2000 年 的 DARPA 城市 挑战 赛 中 获得 第 二 名 ， 其 中 就 使 用 了 SLAM 系统 。 这 使 得 
SLAM 引起 了 全 世界 的 关注 。 现 在 SLAM 实现 已 经 趋 于 大 众 化 ， 即 便 在 消费 型 机 器 人 吸尘器 中 
都 能 找到 SLAM 的 影子 。 

就 本 章 而 言 ， 我 们 将 重点 介绍 其 在 Roban 中 用 于 机 器 人 构图 和 导航 技术 的 应 用 。 


4.2 图 像 的 接收 和 发 布 


在 Roban 机 器 人 中 ， 搭 载 了 Intel 公司 在 2018 年 1 月 推出 的 RealsenseD435 深度 相机 。 为 
此 我 们 安装 了 Intel Realsense SDK2.0 作为 D435 相机 的 驱动 。 包 img publisher 实现 了 图 像 接 
收 和 发 布 的 主要 功能 。 先 通过 D435 接收 相机 数据 ， 然 后 将 深度 图 和 RGB 图 像 对 齐 ， 之 后 使 用 
cv::bridge 将 图 像 转换 为 ROS 消息 ， 同 时 生成 当前 相机 坐标 系 下 的 点 云 数 据 。 最 终 建 立 节 点 并 发 
送 消息 到 话题 中 。 


4.2.1 ”初始 化 和 配置 


在 主要 功能 实现 之 前 ， 我 们 进行 了 函数 、 变 量 的 初始 化 、 相 机 的 配置 ， 以 及 和 ROS 相关 的 
配置 。 

1. 函数 、 参 数 的 声明 及 初始 化 

在 进入 main) 函数 之 前 ， 声 明 函 数 、 初 始 化 变量 ; 定义 常量 ， 定 义 点 云 类 型 。 


// 相机 图 像 接收 频率 
#define FPS 30 


typedef pcl::PointXYZRGB PointT; 


typedef pcl::PointCloud<PointT> PointCloud; 


// 获取 深度 像素 对 应 长 度 单 位 转换 


float get depth scale(rs2::device dev); 


// 检查 摄像 头 数据 管道 设置 是 否 改变 
bool profile changed(const std::vector«rs2::stream profile»& current, const std:: 


vector<rs2::stream_profile>& prev); 


float m invalid depth value. - 0.0; 


float m max z  - 8.0; 
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2. 相机 管道 配置 以 及 深度 图 像 向 RGB 图 像 的 对 齐 
在 Realsense SDK2.0 中 ， 通 过 管道 获取 相机 的 RGB 帧 和 深度 帧 。 所 以 初始 化 时 ， 我 们 配置 
了 两 个 数据 流 一 一 16 位 单 通道 的 深度 数据 流 和 8 位 三 通道 的 RGB 数据 流 ， 以 30Hz 的 接收 频率 
接收 数据 。 
// 创建 一 个 管道 以 及 管道 的 参数 变量 


rs2::pipeline pipe; 


rs2::config p_config; 


// 配置 管道 以 及 启动 相机 

p_config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, FPS); 
p_config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2 FORMAT RGB8, FPS); 
rs2::pipeline_profile profile = pipe.start(p config) ; 


因为 每 个 深度 相机 的 深度 像素 单位 可 能 不 同 ， 因 此 我 们 在 这 里 获取 它 : 


// 使 用 数据 管道 的 profile 获 取 深 度 图 像 像素 对 应 于 长 度 单位 〈 米 ) 的 转换 比例 
float depth_scale = get_depth_scale(profile.get_device()); 


在 此 ， 需 要 声明 一 个 能 够 实现 深度 图 向 其 他 图 像 对 齐 的 rs2::align 类 型 的 变量 align， 在 后 续 
的 代码 中 ， 将 通过 此 变量 实现 深度 帧 的 对 齐 。 
// "align_to" 是 用 深度 图 像 对 齐 的 图 像 流 
// 选择 RGB 图 像 数 据 流 作为 对 齐 对 象 
rs2_stream align_to = RS2_STREAM_COLOR; 
//rs2::align 允许 实现 深度 图 像 对 齐 的 其 他 图 像 
rs2::align align(align_to) ; 


3. 相机 内 参 、 外 参 的 获取 
在 Realsense SDK2.0 中 ， 有 直接 获取 相机 内 参 、 外 参 的 接口 : 


// 声明 数据 流 

auto depth stream = profile.get stream(RS2 STREAM DEPTH).as«rs2::video stream profile 
20; 

auto color stream = profile.get stream(RS2 STREAM. COLOR).as«rs2::video stream profile 
>0; 


// 获取 深度 相机 内 参 


rs2 intrinsics m_depth_intrinsics_ = depth stream.get intrinsics(); 
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// 获取 RGB 相机 内 参 

rs2_intrinsics m_color_intrinsics_ = color stream.get intrinsics(); 

// 获取 深度 相机 相对 于 RGB 相机 的 外 参 ， 即 变换 矩阵 

rs2_extrinsics m_depth_2_color_extrinsics_ = depth_stream.get_extrinsics_to 
(color_stream) ; 

// 获取 RGB 帧 的 长 宽 


auto color_width_ = m_color_intrinsics_.width; 


auto color_height_ = m_color_intrinsics_.height; 


4. ROS 相关 的 配置 


ros::init(argc, argv, "image. publisher"); 


// ROS 节 点 声明 

ros::NodeHandle nh; 

image_transport::ImageTransport it (nh); 

image_transport::Publisher rgbPub = it.advertise("camera/rgb/image_raw", 1); 

image_transport::Publisher depthPub=it.advertise("camera/depth_registered/image_raw", 
1); 

ros::Publisher pointcloud publisher, = nh.advertise<sensor_msgs: :PointCloud2> 


("cloud_in", 1); 


// 图 像 消 息 声明 

sensor_msgs::ImagePtr rgbMsg, depthMsg; 

std msgs::Header imgHeader = std msgs::Header(); 

// 点 云 消息 声明 

PointCloud::Ptr pointcloud_ = boost::make_shared< PointCloud >( ); 


sensor msgs::PointCloud2 msg_pointcloud; 


4.2.2 ”主要 功能 实现 

由 于 需要 实时 持续 获取 相机 数据 , 我 们 将 主要 功能 代码 写 在 while 循环 中 , 只 有 当 节 点 关闭 
时 跳出 循环 。 

1. 帧 的 获取 以 及 深度 帧 的 对 齐 


当 有 图 像 帧 被 接收 时 ，wait_for_frames() 函数 返回 图 像 帧 到 frameset 变量 中 ， 之 后 我 们 分 
别 获 取 RGB 图 像 帧 和 深度 图 像 帧 。 在 判断 过 摄像 头 数 据 管道 设置 没有 改变 之 后 ， 利 用 align 将 
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深度 图 像 对 齐 到 RGB 图 像 上 面 并 且 得 到 对 齐 之 后 的 processed 变量 。 


me frameset = pipe.wait for frames(); 

注意 ， 如 果 此 时 加 上 apply\_filter(rs2::colorizer c) 色 彩 滤 波 器 ， 在 RVIZ 中 可 以 直接 看 到 彩色 的 
深度 图 像 ， 

但 是 如 果 将 此 图 像 作 为 ROS 消 息 传送 到 RGB-D 后 ， 无 法 得 到 理想 的 点 云图 。 

const rs2::frame &color_frame = frameset.get color frame(); 


const rs2::frame &depth frame = frameset.get depth frame(); 


auto color format, = color. frame.as«rs2::video frame»().get profile().format(); 

auto swap rgb. - color format. == RS2 FORMAT BGR8 || color format  -- 
RS2_FORMAT_BGRA8 ; 

auto nb_color_pixel_ = (color_format_ == RS2_FORMAT_RGB8 || color format, == 
RS2 FORMAT BGR8) ? 3 : 4; 


因为 rs2: :align 正在 对 齐 深度 图 像 到 其 他 图 像 流 ， 要 确保 对 齐 的 图 像 流 不 发 生 改 变 
if (profile_changed(pipe.get_active_profile().get_streams(), profile.get streams())) 


1 
Std::cout««"changed?"««std: :endl; 
// 如 果 profile 发 生 改变 ， 则 更 新 align 对 象 ， 重 新 获取 深度 图 像 像 素 到 长 度 单位 的 转换 比例 
profile = pipe.get_active_profile(); 
align = rs2::align(align to); 
depth scale - get depth scale(profile.get device()); 
} 
// 获取 对 齐 后 的 帧 


rs2::frameset processed = align.process(frameset); 


// 尝试 获取 对 齐 后 的 深度 图 像 帧 和 其 他 由 
rs2::frame aligned color frame = processed.get color frame(); 


rs2::frame aligned depth frame = processed.get depth frame(); // apply filter(c) 


// 获取 图 像 的 宽 和 高 
const int depth w = aligned_depth_frame.as<rs2::video_frame>().get_width() ; 
const int depth h = aligned depth. frame.as«rs2::video frame»().get height(); 
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const int color w = aligned_color_frame.as<rs2::video_frame>().get_width() ; 


Ml 


const int color_h = aligned. color. frame.as«rs2::video frame»().get height(; 


2. RGB、 深 度 图 像 消息 发 布 
发 布 时 ，RGB 图 像 为 8 位 3 通道 (CV_8UC3) ， 名 为 rgb8; 深度 图 像 为 16 位 单 通道 
(CV_16UC1), 4H 16UCI. 


// RHA AR 


imgHeader.stamp = ros::Time: :now(); 


// RGB 图 像 

cv::Mat aligned_color_image(cv::Size(color_w,color_h), CV_8UC3, (void*) 
aligned_color_frame.get_data(), cv::Mat::AUTO_STEP) ; 

rgbMsg = cv bridge::CvImage(imgHeader, "rgb8", aligned_color_image) .toImageMsg() ; 

rgbPub. publish (rgbMsg) ; 


// 深度 图 像 
cv::Mat aligned depth, image(cv::Size(depth w,depth h), CV_16UC1, (void*) 
aligned depth. frame.get data(), cv::Mat::AUTO. STEP) ; 
depthMsg = cv bridge::CvImage(imgHeader, "16UCi", aligned depth, image) .toImageMsg(); 
depthPub.publish(depthMsg); 


图 4.1 是 RGB 图 和 深度 图 示例 。 
3. 点 云图 像 发 布 


点 云图 像 是 基于 函数 rs2_deproject_pixel to point() 生成 的 。 其 中 运用 了 和 针 孔 相机 成 像 的 转 
换 矩 阵 。 在 位 置 坐标 赋值 之 后 ， 又 将 相对 应 的 颜色 坐标 赋值 到 对 应 的 点 上 ， 从 而 能 够 得 到 彩色 
的 点 云图 。 


// Get the depth value of the current pixel 

auto pixels distance = depth scale *p. depth, frame[depth, pixel index]; 
float depth. point[3]; 

const float pixel[] = {(float)j, (float)i); 

rs2 deproject. pixel to point(depth point, &m depth intrinsics,, pixel, 


pixels distance); 
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float color_point [3]; 
rs2_transform_point_to_point(color_point, &m_depth_2_color_extrinsics_, depth_point) ; 
float color. pixel[2]; 


rs2 project point to pixel(color. pixel, &m color intrinsics., color. point); 


41 RGB 图 和 深度 图 示例 


图 4.2 是 点 云图 示例 。 


图 4.2 点 云图 示例 


至 此 ， 我 们 可 以 通过 img_package 包 来 发 布 RGB 图 、 深 度 图 以 及 点 云图 的 消息 了 。 此 时 的 
rqt_graph 界面 如 图 4.3 所 示 。 
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/camera/depth registered/image raw 


/camera/rgb/image raw 


/image publisher 


4.3 rqt graph 界面 
4.3 ”定位 和 图 像 追 踪 一 一 ORB-SLAM2 


这 里 我 们 采用 ORB-SLAM2 包 来 实现 此 功能 。ORB-SLAM2 是 一 个 服务 于 单 目 、 双 目 和 
RGB-D 相机 的 完整 的 SLAM 系统 ， 包 括 地 图 重用 、 闭 环 和 重 定位 的 功能 。 该 系统 可 在 各 种 环境 
中 的 标准 CPU 上 实时 工作 ， 从 小 型 手持 室内 序列 到 在 工业 环境 中 飞行 的 无 人 机 以 及 在 城市 周转 
行驶 的 汽车 。 对 于 一 个 未 知 区 域 ， 该 系统 能 从 视觉 上 进行 简单 的 定位 以 及 测量 追踪 。 


4.3.1 ”数据 接收 和 程序 初始 化 


ORB-SLAM2 系统 对 于 RGB-D 相机 的 处 理 ， 采 用 了 光束 法 平 差 方 法 (BA)， 从 而 实现 了 精 
确 度 的 最 大 化 以 及 深度 误差 的 最 小 化 ， 其 中 可 执行 程序 代码 主体 位 于 ros_rgbd.cc 文件 中 。 在 数 
据 处 理 之 前 ， 需 要 调整 Realsense D435 相机 在 文件 rgbd.yaml 中 的 参数 。 以 下 是 具体 的 参数 以 及 
获取 方法 (可 以 通过 相机 标定 获取 .yaml 文件 中 的 对 应 参数 ): 


# 可 以 通过 img_publisher 获 取 相 机 内 参 、 外 参 ， 以 及 图 像 的 像素 长 宽 
# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 606.437 

Camera.fy: 605.259 

Camera.cx: 318.563 

Camera.cy: 269.261 

Camera.width: 640 

Camera.height: 480 


# Camera frames per second 


Camera.fps: 30.0 


# IR projector baseline times fx (aprox.) 
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Camera.bf: 40.0 
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 
Camera.RGB: 1 


# 通常 是 50， 不 需要 改 
# Close/Far threshold. Baseline times 
ThDepth: 50 


# 因为 在 传输 时 相机 的 比例 为 0.001 ,所 以 此 处 的 深度 系数 要 选择 1000 
# Deptmap values factor 
DepthMapFactor: 1000 


# 以 下 不 需要 改动 


首先 创建 SLAM 系统 实例 并 初始 化 所 有 系统 进程 : 


// 创建 SLAM 系 统 实例 并 初始 化 所 有 的 系统 进程 ， 做 好 处 理 帧 的 准备 
ORB_SLAM2: :System SLAM(argv[1] ,argv [2] , ORB. SLAM2: :System: :RGBD ,bUseViewer ,bReuseMap) ; 


ORB-SLAMO 的 进程 主要 包括 ; 

(1) TRACKING: 通过 找到 各 帧 之 间 的 特征 来 定位 相机 位 置 并 实行 跟踪 。 

(2) LOCAL MAPPING: 管理 本 地 地 图 并 进行 优化 (调用 本 地 BA). 

(3) LOOP CLOSING: 通过 执行 姿势 图 优化 来 检测 大 回路 并 校正 累积 的 漂移 。 
在 ORB_SLAM2::System 的 构造 函数 中 ， 有 这 3 个 进程 的 调用 : 


//Initialize the Tracking thread 
//(it will live in the main thread of execution,the one that called this constructor) 
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, 


mpPointCloudMapping, mpKeyFrameDatabase, strSettingsFile, mSensor, bReuse) ; 


//Initialize the Local Mapping thread and launch 
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR) ; 
mptLocalMapping = new thread(&ORB. SLAM2: :LocalMapping: :Run,mpLocalMapper) ; 
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//Initialize the Loop Closing thread and launch 

mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!= 
MONOCULAR) ; 

mptLoopClosing = new thread(&O0RB_SLAM2::LoopClosing::Run, mpLoopCloser) ; 


43.2 ”点 云 地 图 创建 /重用 

在 开始 之 前 ， 需 要 选择 创建 新 地 图 还 是 重新 调用 地 图 。 

1. 点 云 地 图 创建 

当 我 们 到 一 个 新 的 环境 中 时 ， 需 要 获取 周围 环境 的 地 图 并 存储 为 pcd 文件 ， 此 时 在 执行 语 
句 的 最 后 选择 false: 


rosrun SLAM RGBD utils/ORBvoc.bin utils/rgbd.yaml true false 


此 时 在 System 类 中 ， 将 创建 新 的 地 图 变量 : 


mpMap = new Map(); E 


“4 RGB-D 程序 结束 时 ， 我 们 期 望 把 所 挑选 出 的 关键 帧 打印 成 一 个 连续 的 点 云 地 图 。 在 打印 
生成 的 点 云图 像 时 ， 我 们 应 在 文件 PointCloudMapping.cc 的 generatePointCloud() 函数 里 面 做 好 
相应 参数 的 调整 : 


if (d « 0.05 || d > 20) 
continue; 


还 要 在 PointCloudMapping 类 的 构造 函数 中 适当 调整 树叶 的 大 小 : 


this->resolution = 0.01; | 


若 出 现在 相机 完成 闭环 时 段 的 错误 , 是 因为 在 CorrectLoopO 中 使 用 了 一 个 叫 KeyFrame And- 
Pose 的 数据 类 型 ， 这 个 数据 结构 中 会 创建 Eigen 对 象 ， 根 据 Eigen 文档 ， 调 用 Eigen 对 象 的 数据 
结构 需要 使 用 Eigen 的 字 节 对 齐 ， 所 以 在 LoopClosing.h 中 加 六 此 宏 定 义 语句 ， 


class LoopClosing 
{ 
public: 


EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 加 入 此 语句 
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typedef pair<set<KeyFrame*>,int> ConsistentGroup; 


}; 


至 此 ， 我 们 获得 了 清晰 可 见 的 点 云图 pointcloud.pcd。 图 4.4 为 办 公 室 走廊 及 办 公 桌 附近 的 
点 云图 演示 。 


图 4.4 走廊 及 办 公 桌 点 云图 


2. 点 云 地 图 重用 (reuse) 
当 我 们 已 经 有 了 当前 环境 的 地 图 ， 在 需要 调用 此 地 图 时 ， 我 们 在 执行 语句 的 最 后 选择 true: 


rosrun SLAM RGBD utils/ORBvoc.bin utils/rgbd.yaml true true 


此 时 ， 在 系统 的 构造 函数 中 进行 原 有 地 图 的 加 载 工作 : 


LoadMap("Slam_Map.bin") ; 


// mpKeyFrameDatabase->set_vocab(mpVocabulary) ; 


ector<ORB_SLAM2: :KeyFrame*> vpKFs = mpMap->GetAllKeyFrames() ; 
for (vector<ORB_SLAM2: :KeyFrame*>::iterator it = vpKFs.begin(); it != vpKFs.end(); + | 
it) { 
(*it) ->SetKeyFrameDatabase (mpKeyFrameDatabase) ; 
(*it) ->SetORBvocabulary (mpVocabulary) ; 
(*it) ->SetMap (mpMap) ; 
(*it) ->ComputeBow() ; 
mpKeyFrameDatabase->add(*it) ; 


(*it) ->SetMapPoints (mpMap-»GetAllMapPoints()); 
(*it) ->SetSpanningTree(vpKFs) ; 
(*it) ->SetGridParams (vpKFs) ; 
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// Reconstruct map points Observation 
} 
| 
vector<ORB_SLAM2: :MapPoint*> vpMPs = mpMap-»GetAllMapPoints(); 
| for (vector«ORB SLAM2::MapPoint*^::iterator mit = vpMPs.begin(); mit !- vpMPs.end(); 
++mit) { 
(*mit)-»SetMap(mpMap); 
(*mit)-»SetÜbservations(vpKFs); 


for (vector<ORB_SLAM2: :KeyFrame*>::iterator it = vpKFs.begin(); it != vpKFs.end(); ++ 
it) { | 


(*it)->UpdateConnections(); 


我 们 获取 并 将 带 有 时 间 戳 的 实时 位 置 以 geometry_msgs::PoseWithCovarianceStamped 的 消息 
发 布 出 去 。 

至 此 ， 如 图 4.5 所 示 ， 我 们 将 image_publisher 节点 发 布 的 RGB 图 以 及 深度 图 进行 处 理 ， 得 
到 了 PCD 地 图 文件 以 及 实时 发 布 的 当前 位 置 坐 标 。 


/camera/depth registered/image raw 


/RGBD 
/image publisher /camera/rgb/image raw 


图 4.5 SLAM 节点 结构 


如 图 4.6 所 示 ， 在 RGB 图 片 中 ， 紫 色 的 点 代表 原来 地 图 上 已 经 有 的 关键 点 ;绿色 的 点 代表 
重新 识别 出 来 的 关键 点 。 

如 图 4.7 所 示 ， 在 点 云图 中 ， 绿 色 的 方 框 代表 当前 位 置 ， 红色 的 点 云 代 表 当 前 RGB 图 像 在 
点 云图 上 的 位 置 ， 黑色 的 点 代表 已 经 识别 的 点 云 。 
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4.6 ”处 理 后 的 RGB 


47 处理 后 的 点 云图 


4.4 ” 八 又 树 图 层 的 截取 以 及 平面 地 图 的 生成 
4.4.1 ” 八 叉 树 图 层 的 截取 


为 了 方便 系统 存储 和 处 理 ， 使 用 八 又 树 的 方法 进行 数学 建 模 ， 大 大 减 小 了 系统 的 存储 空间 
以 及 缩短 了 处 理 时 长 。 我 们 使 用 octomap_sever 包 去 生成 八 又 树 图 。octomap_sever 能 加 载 3D 地 
图 ， 并 以 紧凑 的 二 进 制 格式 将 其 分 发 到 其 他 节点 。 它 还 允许 增 量 构建 3D OctoMaps， 并 在 节点 
octomap_sever 中 提供 地 图 保存 。 

当 八 叉 树 图 生成 之 后 ， 需 要 对 生成 的 八 又 树 进行 截取 处 理 ， 去 掉 地 面 且 去 掉 高 于 Roban 机 
器 人 身高 的 部 分 。 首 先 借助 octomap_sever 中 的 静态 八 又 树 追 踊 的 roslaunch， 如 图 4.8 所 示 ， 我 
们 可 以 直接 将 点 云 的 三 进 制 文件 转换 成 八 叉 树 。 在 转换 的 过 程 中 需要 自行 配置 参数 ， 以 截取 合 
适 的 图 层 。 例 如 ， 通 过 考虑 机 器 人 的 身高 以 及 去 除 地 面 的 影响 ， 我 们 所 选 的 roslaunch 参数 如 下 : 
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<param name="occupancy_max_z" value="0.11"/> 


<param name-"occupancy min z" value="-0.41"/> 


图 4.8 截取 前 的 八 又 树 图 
之 后 ， 如 图 4.9 所 示 ， 我 们 能 够 得 到 截取 之 后 的 八 又 树 图 。 


图 4.9 截取 后 的 八 又 树 图 


4.4.2 ”平面 地 图 的 生成 
与 此 同时 ， 我 们 可 以 在 map 主题 中 订阅 生成 的 平面 地 图 ， 如 图 4.10 所 示 。 


图 4.10 生成 的 平面 图 


iadaa i 


4.10 中 的 黑色 部 分 代表 不 可 进入 的 区 域 ， 白色 部 分 代表 镜头 朝向 (由 于 加 载 的 是 静态 地 
图 ， 此 区 域 不 会 随 着 当前 镜头 的 移动 而 改变 ， 是 由 创建 地 图 时 最 后 的 镜头 位 置 所 决定 的 ); 深 绿 
色 部 分 代表 可 以 进入 的 区 域 。 至此， 如 图 4.11 所 示 ， 得 到 了 可 以 进行 路 径 分 析 的 平面 地 图 ， 同 
时 主题 /map 由 octomap_talker 节点 发 布 。 


/camera/depth registered/image raw 
[image publisher 


/camera/rgb/image raw 


4.41 rqt graph 界面 


4.5 ”路 径 规划 


路 径 规 划 是 通过 humanoid_planner_2d 包 来 实现 的 ,此 package 能 订阅 当前 位 置信 息 /initialpose 
二 维 的 平面 图 /map 以 及 给 定 的 目标 点 /mov_base_simple/goal, 然后 根据 这 些 信息 将 到 达 目 标点 最 
短 的 路 径 画 出 来 。 实 现 结果 如 图 4.12 所 示 。 


图 4.12 ”路径 规划 


但 是 ， 在 实际 运行 时 发 现 ， 当 机 器 人 行走 经 过 障碍 物 时 ， 如 果 按 照 当 前 路 径 一 一 只 追求 路 
径 最 短 而 太 靠近 障碍 物 ， 会 导致 机 器 人 不 能 较 好 地 躲避 障碍 物 。 所 以 我 们 希望 机 器 人 经 过 路 径 
最 短 的 前 担 下 ， 尽 量 远离 障碍 物 。 因 此 ， 我 们 需要 把 可 以 经 过 的 部 分 按照 距离 障碍 物 的 远近 计 
算 权 重 ， 距 离 障碍 物 越 近 ， 权 重 就 越 大 。 以 下 是 代码 的 核心 部 分 : 


const int SHADOW_RADIUS = 8; 
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Nec) 


// 将 本 地 地 图 初始 化 
for(unsigned int j = 0; j < mapHeight; ++j) 
for(unsigned int i = 0; i < mapWidth; ++i) 
GridLocal [i] [j]=0; 
// 将 地 图 划分 权重 
for(unsigned int j = 0; j < mapHeight; ++j){ 
for(unsigned int i = 0; i < mapWidth; ++i){ 
if (map_->isOccupiedAtCell(i,j)) { | 
GridLocal[i][j]-OBSTACLE .COST; 
for(int k = 0; k < SHADOW RADIUS; k++) { 
if((i-k >= 0) && (i-k < mapWidth) && (map. -»isÜccupiedAtCell(i-k,j))-- 
false) { 
GridLocal[i-k][j] += (SHADOW RADIUS-k); 
if (GridLocal [i-k] [j]>=SHADOW_RADIUS) 
GridLocal [i-k] [j]=SHADOW_RADIUS ; 


if((itk >= 0) && (i+k < mapWidth) && (map_->isOccupiedAtCell (i+k, j))== 
false) { 
GridLocal [it+k] [j]+=SHADOW_RADIUS-k; 
if (GridLocal [i+k] [j]>=SHADOW_RADIUS) 
GridLocal [itk] [j]=SHADOW_RADIUS ; 


if((j-k >= 0) && (j-k < mapHeight) && (map_->isOccupiedAtCell1 (i, j-k)) 
==false) { 
GridLocal [i] [j-k]+=SHADOW_RADIUS-k; 
if (GridLocal [i] [j-k] >=SHADOW_RADIUS) 
GridLocal [i] [j-k]=SHADOW_RADIUS ; 


if((j*k >= 0) && (j+k < mapHeight) && (map_->isOccupiedAtCell(i, j+k)) 
==false) { 
GridLocal [i] [j+k]+=SHADOW_RADIUS-k; 
if (GridLocal [i] [j+k] >=SHADOW_RADIUS) 
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GridLocal [i] [j+k]=SHADOW_RADIUS ; 


首先 设 定 一 个 常量 SHADOW_RADIUS, 代表 地 图 障碍 物 所 能 影响 到 的 范围 。 我 们 从 地 图 的 
(0,0) 位 置 开始 , 以 此 对 地 图 像素 点 进行 分 析 。 若 此 点 已 被 占用 , 则 给 它 赋 值 为 OBSTACLE_COST 
(障碍 物 )。 如 果 此 点 未 被 占用 ， 紧 接着 我 们 便 从 此 点 出 发 ， 以 SHADOW_RADIUS 的 距离 范围 
向 上 、 下 、 左 、 右 4 个 方向 进行 迭代 ， 次 数 为 设 定 的 影子 半径 : 若 在 影子 范围 内 仍然 是 障碍 物 ， 
则 不 做 处 理 ; 反之 ， 则 根据 距离 起 始点 的 距离 设 定 权 重 一 一 距离 越 近 ， 权 重 越 大 。 结 果 显 而 易 
见 ， 当 通过 两 个 障碍 物 的 过 道 时 ， 必 然 会 从 中 间 穿 过 以 保证 机 器 人 所 经 过 的 路 径 为 权重 最 小 的 。 
图 4.13 和 图 4.14 为 两 个 实际 路 径 规划 效果 图 。 


图 4.14 优化 之 后 的 路 径 2 


实现 结果 如 图 4.15 所 示 。 
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move base simple/goal 
camera/depth _ RE raw 
— /initialpose humanoid planner 2d 
image publis [/camera/rgb/image. raw] ee E Cu pe cn 01 4900581507508 n rviz 1590148065413075308 Ses occupled_cells_vis_arra: 区 
|/cloud. in| |/cloud. in| ([octomap. vali ([octomap. vali 


Æ 4.15 rqt graph 界面 
4.6 行走 实现 
之 后 ， 我 们 将 获得 的 路 径 传 入 path_controler 包 来 实现 机 器 人 的 行走 。 此 包 的 主要 功能 是 进 
一 步 分 析 接 收 到 的 路 径 信息 ， 以 及 向 机 器 人 发 布 行走 的 信息 。 
4.6.1 ”路 径 分 析 


首先 ,在 接收 到 nav_msgs::Path 类 型 的 路 径 信息 之 后 ， 我 们 提取 出 其 中 的 数据 信息 ， 可 以 获 
得 一 串 含有 多 个 点 的 位 置信 息 。 其 中 每 个 点 包含 3 个 位 置信 息 x、y、z。 其 中 我 们 只 取 x 和 y 分 
量 作 为 此 路 径 点 在 地 图 上 的 位 置 : 


req path. controler sub = nh.subscribe<std_msgs: :Bool>("requestGaitCommand", 1, 
&path_controlerCallback) ; 


同时 ， 我 们 通过 startCallback 回调 函数 来 接收 来 自 /initialpose 话题 的 当前 位 置信 息 : 


// globoal 


geometry msgs::Pose start pose. ; 


// main 
start sub. = nh.subscribe<geometry_msgs: :PoseWithCovarianceStamped>("initialpose", 1, 
&startCallback) ; 


// function startCallback 

void startCallback(const geometry_msgs: :PoseWithCovarianceStampedConstPtr 
&start_pose) 

{ 

// set start: 

start_pose_ = start_pose->pose.pose; 


5 


108 i 仿 人 机 器 人 建 模 与 控制 


geometry_msgs::Pose 类 型 的 全 局 变量 start_pose_ 代 表 实 时 的 位 置信 息 。 除 了 可 以 直接 接收 
当前 的 位 置信 息 以 外 ， 变 量 start_pose_ 还 包含 着 机 器 人 的 以 四 元 数 方法 表示 的 形态 信息 : x. ys 
z、w。 根 据 四 元 数 与 欧 拉 角 的 转换 矩阵 可 得 ， 偏 航 角 y 与 四 元 数 的 关系 式 为 


2 (gogs + 9192) 


4.1 
1— 2(a$ + q2) dd 


^y = arctan 


因此 ， 在 pathCallback 回调 函数 中 ， 我 们 使 用 如 下 代码 进行 当前 方向 信息 的 提取 : 


cout << "pathcomming" << endl; 
if (!walkFinish) 

return; 
reachGoal = (path.poses.size() == 1) ? 1 : 0; 
if (reachGoal) 


return; 


cout << "pathanalysis" << endl; 


double initX = path.poses[0] .pose.position.x; 
double initY = path.poses[0].pose.position.y; 


double q0 = start pose .orientation.w; 
double qi = start pose .orientation.x; 
double q2 = start pose, .orientation.y; 
double q3 = start pose. .orientation.z; 
double rotaDegree = 57.29 * atan2(2*(q0*q3*gi*q2), 1-2*(q3*q3 + q2*q2)); 


其 中 , 变量 walkFinish 表示 当前 行走 是 否 结束 ; 变量 reachGoal 则 表示 是 否 到 达 路 径 的 终点 。 
在 有 了 当前 的 位 置 坐标 和 朝向 ， 再 加 上 规划 好 的 路 径 信息 ， 我 们 便 可 以 分 析 路 径 ， 获 得 真正 符 
合 机 器 人 行走 的 线路 。 代 码 如 下 : 


double tmpX1, tmpY1; 


float distance, goalRota, goalRotaDegree, deltaDegree; 


int stepCounter = 0; 


for (int i = 0; i < 5; i++) 
stepDistance[i] = stepDegree[i] = 0; 


cout << "Solution,size:" << path.poses.size() << endl; 
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for (int i = 1; i < path.poses.size() - 1; i++) 


{ 


tmpXi 
tmpYi 


path.poses[i].pose.position.x; 


path.poses[i].pose.position.y; 


distance = sqrt((tmpXi - initX) * (tmpXi - initX) + (tmpY1 - initY) * (tmpY1 - 
initY)); 

goalRota = atan2(tmpY1 - initY, tmpX1 - initX); 

goalRotaDegree = (float)(goalRota * 57.29); 

deltaDegree - goalRotaDegree - rotaDegree; 


if (deltaDegree « -180) 
1 
deltaDegree *- 360; 


else if (deltaDegree » 180) 


deltaDegree -- 360; 


if (stepCounter < 5 && (distance > 0.11)) 


cout << "addstep -> "; 
cout << "distance: " << distance << ", " 
<< "deltaDegree: " «« deltaDegree «« endl; 

stepDistance[stepCounter] - distance; 
stepDegree[stepCounter] = deltaDegree; 
if (++stepCounter == 5) 

break; 
initX = tmpX1; 
initY = tmpY1; 
rotaDegree = goalRotaDegree; 


} 
walkFinish = 0; 
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我 们 希望 将 地 图 上 的 路 径 信息 转换 成 可 以 直接 作用 于 机 器 人 的 例如 右 转 40"， 直 行 一 步 的 
信息 ， 首 先 我 们 通过 初始 机 器 人 方向 以 及 当前 为 止 指向 第 一 个 目标 点 的 方向 可 以 计算 出 在 原 地 
的 转角 , 这 便 得 到 了 第 一 个 转动 的 角度 。 因 为 我 们 认为 在 后 面 的 行走 中 , 每 走 完 一 步 以 后 的 朝向 
是 沿 着 上 一 个 在 路 径 信息 的 分 析 过 程 中 ， 我 们 希望 地 图 上 的 路 径 信息 可 以 转换 成 简单 明了 的 能 
够 使 机 器 人 前 进 或 者 转弯 的 信息 。 基 于 这 个 目的 ， 我 们 的 路 径 分 析 过 程 旨 在 输出 两 个 数组 ， 一 
个 数组 是 每 一 步 所 走 的 长 度 ， 另 一 个 数组 是 每 一 步 所 旋转 的 角度 。 其 中 ， 在 求 初 始 转动 的 角度 
与 其 他 的 转角 不 同 ， 需 要 通过 当前 的 朝向 来 求 。 我 们 计算 当前 位 置 的 方向 与 当前 位 置 指向 下 一 
个 目标 点 的 差 ， 作 为 第 一 次 旋转 的 角度 、 另 外 ， 我 们 添加 了 每 一 步 的 步 长 不 应 该 小 于 0.11 的 限 
定 。 首 先是 因为 路 径 中 的 目标 点 过 于 密集 ， 不 适合 将 目标 点 直接 作为 行走 的 点 ; 其 次 是 因为 如 
果 步 长 太 小 ， 将 不 能 忽略 路 径 中 存在 的 锯齿 形 误差 ， 从 而 影响 机 器 人 正常 的 前 进 。 所 以 ， 我 们 
将 满 5 次 且 每 一 次 步 长 大 于 0.11 的 行走 的 点 发 送出 去 ， 作 为 一 次 行走 ， 同 时 将 walkFinish 参数 
设置 为 0 以 代表 开始 这 一 次 行走 。 


4.6.2 ”行走 控制 


行走 控制 直接 和 Roban 机 器 人 的 步 态 控制 挂钩 ， 它 接收 来 自 BodyHubeNode 节点 的 请 求 信 
息 后 ， 发 送 相应 的 行走 信息 以 实现 机 器 人 行走 。 代 码 如 下 : 


// main 


req path controler sub = nh.subscribe<std_msgs::Bool>("requestGaitCommand", 1, 
& path, controlerCallback); 


// path_controlerCallback gaitCommand 
void path, controlerCallback(const std msgs::Bool::ConstPtr &req) 
1 

if (reachGoal && walkFinish) 


return; 


std msgs::Float64MultiArray gaitComm; 
gaitComm.data.resize(3); 

if (req->data == 1) 

1 


cout << "req command" << endl; 


if (countCommand < STEP) 
{ 


第 4 章 “ 同 上 证 位 与 地 图 构建 让 101 


if ((!walkFinish) && (!reachGoal)) 


{ 
if (holdOn == 0 && FLAG == false) 
{ 
if (fabs(stepDegree[countCommand]) > 20) 
hold0n = fabs(stepDegree[countCommand]) / 8; 
FLAG = true; 
} 
if (holdOn > 0) 
{ 
cout << "seulement,tourner,," << stepDegree[countCommand] << endl; 
for (int j = 0} j < 3; j++) 
{ 
if (stepDegree[countCommand] > 0) 
gaitComm.data[j] = Z command[jl; 
else 
gaitComm.data[j] = C_command[j]; 
} 
path_controler_pub.publish(gaitComm) ; 
holdÜn--; 
if (holdOn == 0) 
countCommand = 5; 
Y 
else if (holdOn == 0 && stepDistance[countCommand] >= 0.1) 
t 


for (int j = 0; j < 3; j++) 
{ 
gaitComm.data[j] = W_command[j]; 


path_controler_pub.publish(gaitComm) ; 
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countCommand++ ; 
FLAG = false; 
} 
Ph 
} 
else 
{ 
cout << "sleeping" << endl; 
sleep(7) ; 
FLAG = false; 
countCommand = 0; 
walkFinish = 1; 
} 
} 
} 


到 此 为 止 ， 我 们 可 以 通过 img_package 包 来 发 布 RGB 图 、 深 度 图 以 及 点 云图 的 消息 了 。 此 
时 的 rqt_graph 界面 如 图 4.16 所 示 。 


hurmanoid 
=)> /path 
lanner 2d = 
Qm e path controle /requestGaitCommand 
/galtCommand : A BodyHubNode 


图 4.16 SLAM 算法 总 体 结构 图 
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V-REP 使 用 概述 


CHAPTER 5 


V-REP(Virtual Robot Experimentation Platform， 又 称 V-REP、CoppeliaSim) 是 一 款 由 瑞士 
Coppelia Robotics 公司 开发 的 支持 多 平台 的 机 器 人 仿真 器 ， 支 持 Windows. Mac OS 和 Linux 
系统 。 它 主要 定位 于 机 器 人 仿真 建 模 领域 ， 可 以 利用 内 婴 肢 本、 插件 、ROS 节点 、 远 程 API 
(Application Programming Interface， 应 用 程序 接口 ) 客户 端 或 者 自 定义 的 解决 方案 等 实现 分 布 
式 的 控制 结构 ， 是 非常 理想 的 机 器 人 仿真 建 模 工具 。V-REP 提供 了 多 种 机 器 人 模型 ， 大 大 便捷 
了 机 器 人 仿真 ， 特 别 是 针对 常见 的 工业 机 器 人 和 移动 机 器 估 ，V-REP 提供 了 许多 可 直接 使 用 的 
仿真 模型 ， 通 过 专用 的 API， 各 种 不 同 功能 可 以 非常 容易 集成 并 组 合 在 一 起 。 控 制 器 可 以 采用 
C/C++, Python, Java, Lua, MATLAB, Octave 或 Urbi 等 语言 实现 。 男 外 ，V-REP 可 以 通过 通 
信 接 口 与 ROS 一 起 运行 。 该 接口 让 我 们 可 以 通过 话题 和 服务 来 控制 仿真 场景 和 机 器 人 ， 完 成 自 
动 化 系统 模拟 、 远 程 监控 、 硬 件 控制 、 安 全 性 检查 和 控制 算法 开发 等 。 

图 5.1 是 官方 提供 的 一 些 机 器 人 模型 。 


5.1 官方 提供 的 机 器 人 模型 
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5.1] V-REP 使 用 简介 


V-REP(CoppeliaSim) 是 机 器 人 仿真 器 里 的 “瑞士 军刀 ”: 你 不 会 发 现 一 个 比 它 拥有 更 多 功 


、 特 色 或 是 更 详尽 应 用 编程 接口 的 机 器 人 仿真 器 。 它 具有 如 下 功能 : 


(1) 路 平台 (Windows, Mac OS, Linux). 
(2) 6 种 编程 方法 (嵌入 式 脚 本 、 插 件 、 附 加 组 件 、ROS 节点 、 远 程 客户 端 应 用 编程 接口 、 


BlueZero 节点 )。 


算 、 


(3) 6 种 编程 语言 (C/C++、Python、Java、Lua、MATLAB 和 Octave). 

(4) 超过 400 种 不 同 的 应 用 编程 接口 函数 。 

(5) 4 个 物理 引擎 CODE. Bullet. Vortex. Newton). 

(6) Integrated ray-tracer(POV-Ray). 

CD 完整 的 运动 学 解 算 器 《对 于 任何 机 构 的 逆 运 动 学 和 正 运动 学 )。 

(8) Mesh, OCtree, point cloud- 网 孔 干 扰 检测 。 

(9) Mesh、OCtree、point cloud- 网 孔 最 短 距离 计算 。 

C10) 路 径 规划 CHE 2~6 维 中 的 完整 约束 、 对 于 车 式 车 辆 的 非 完 整 约束 )。 

GOD 嵌入 图 像 处 理 的 视觉 传感器 (完全 可 拓展 )。 

(12) 现实 的 接近 传感器 (在 检测 区 域 中 的 最 短 距离 计算 )。 

(13) 嵌入 式 的 定制 用 户 接 口 ， 包 括 编 辑 器 。 

(14) 完全 集成 的 第 四 类 Reflexxes 运动 库 + RRS-1 interface specifications. 

(15) 数据 记录 与 可 视 化 (时 距 图 、X/Y 图 或 三 维 曲线 )。 

(16) 整合 图 形 编辑 模式 。 

C17) 支持 水 /气体 喷射 的 动态 颗粒 仿真 。 

C18) 带 有 拖 放 功能 的 模型 浏览 器 (在 仿真 中 依旧 可 行 )。 

(19) 多 层 取 消 / 重 做 、 影 像 记 录 、 油 漆 的 仿真 、 详 尽 的 文档 等 。 

其 广泛 应 用 于 机 器 人 、 机 器 人 学 、 仿 真 器 、 仿 真 、 运 动 学 、 动 力学 、 路 径 规划 、 最 短 距离 计 
碰撞 检测 、 视 觉 传感器 、 图 像 处 理 、 接 近 传 感 器 、 油 漆 分 散 仿真 等 领域 。 

本 节 讲 解 如 何 安装 、 设 置 V-REP， 以 及 如 何 安装 ROS 通信 工具 ， 讨 论 一 些 初级 代码 并 了 解 


其 工作 原理 。 还 将 展示 如 何 使 用 服务 和 话题 与 V-REP 进行 交互 ， 以 及 如 何 导入 和 连接 新 的 机 器 
人 模型 。 


51.1 AS 
在 开始 学 习 V-REP 之 前 ， 我 们 首先 对 V-REP 的 整体 有 一 个 初步 的 认识 ， 其 模块 化 和 分 


布 式 框架 有 助 于 实现 复杂 的 场景 ， 各 种 传感器 和 执行 器 可 以 根据 自己 的 速率 和 特性 同时 异步 地 


ees 


` 


1 


行 。 

1. V-REP 框架 

V-REP 是 围绕 着 多 功能 框架 而 设计 的 ， 具 有 各 种 相对 独立 的 功能 ， 可 以 根据 用 户 的 需要 来 
启用 或 禁用 。 

我 们 想象 一 个 仿真 场景 ， 一 个 工业 机 器 人 要 拿 起 盒子 并 将 它 移 动 到 另 一 个 地 方 。 在 这 种 情 
况 下 ，V-REP 对 抓 取 并 握 住 盒 子 这 一 行为 进行 动力 学 计算 ， 而 对 其 他 可 忽略 力学 效应 的 部 分 进 
行 运动 学 仿真 。 这 种 方法 可 以 快速 、 精 确 地 计算 机 器 人 的 运动 。 

2. 场景 对 象 

V-REP 中 的 实体 包括 场景 对 象 (scene object) 和 集合 〈collection )。 场 景 对 象 是 V-REP 中 用 
于 搭建 仿真 场景 的 主要 元 素 ， 主 要 包括 形状 、 关 节 、 图 、 光 、 摄 像 机 、 镜 子 、 路 径 、 视 觉 传感器 
和 力 传感器 等 。 下 面 简要 介绍 其 中 的 几 个 对 象 及 其 作用 。 

COD 关节 :用 于 连接 两 个 或 多 个 场景 对 象 的 运动 低 副 ， 其 至 少 有 一 个 自由 度 。 

(2) 力 传感器 : 两 个 对 象 之 间 的 刚性 连接 ， 用 于 测量 传递 力 和 力矩 。 

(3) BRE: 用 于 定义 空间 中 的 轨迹 ， 可 以 用 于 路 径 规划 、 机 器 人 末端 执行 的 引导 等 。 

3. 计算 模块 

场景 对 象 通常 不 会 单独 出 现 ， 而 是 有 多 个 对 象 同时 出 现 、 协同 作用 。V-REP 拥有 强大 的 计 
算 功能 ， 并 作用 在 这 些 对 象 上 。V-REP 中 包含 能 实现 快速 干涉 检测 的 冲突 检测 模块 、 用 于 测量 
两 个 实体 间 最 短路 径 的 最 短 距离 计算 模块 、 运 动 学 解 算 模块 、 几 何 约束 求解 模块 、 路 径 和 运动 
规划 模块 以 及 动力 学 模块 。 

动力 学 模块 可 用 于 动态 地 模拟 对 象 或 模型 的 运动 来 实现 它们 之 间 的 相互 作用 (如 碰撞 反应 、 
物体 的 抓 取 等 )， 这 由 V-REP 中 的 4 个 物理 引擎 (Bullet、ODE、Vortex 41 Newton) 来 实现 。 选 
择 合 适 的 物理 引擎 有 助 于 提高 仿真 的 速度 和 精度 。 这 些 计算 模块 使 我 们 能 够 模拟 真实 的 物理 环 
境 ， 为 位 置 求解 、 路 径 设 计 等 提供 了 方便 。 

4. 仿真 控制 方法 

V-REP 支持 多 种 编程 方法 来 控制 仿真 ， 并 且 这 些 方 法 相互 兼容 ， 可 以 同时 使 用 。 

最 便捷 的 方法 是 用 Lua (一 种 高 效 、 小巧、 可 帐 入 的 脚本 语言 ) 编写 子 脚本 即 仿真 脚本 来 控 
制 给 定 的 机 器 人 或 模型 的 行为 。 同 样 ， 也 可 以 通过 编写 插件 来 控制 机 器 人 或 仿真 。 第 三 种 方法 
是 在 远程 API 的 基础 上 编写 外 部 应 用 程序 。 该 方法 轻便 快捷 ， 可 以 从 外 部 应 用 、 机 器 人 或 是 另 
一 人 台 计 算 机 土 通过 运行 控制 代码 来 进行 仿真 ， 以 确保 该 代码 和 实际 控制 机 器 人 的 代码 的 同一 性 。 
目前 ，V-REP 支持 C/C++, Python, Java. MATLAB 等 几 种 编程 语言 。 

ROS 节点 的 执行 机 制 和 远程 API 类 似 , Ait ROS 节点 可 以 连接 多 个 进程 , 而且 有 大 量 的 可 
兼容 的 库 可 使 用 。 我 们 也 可 以 通过 编写 外 部 应 用 程序 来 控制 仿真 ， 通 过 API 或 者 串口 等 方式 与 
V-REP 脚本 或 插件 实现 通信 。 


(i 
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5.1.2 ”安装 带 有 ROS 的 V-REP 


在 开始 使 用 V-REP 之 前 , 需要 在 系统 中 安装 它 , 并 且 编译 相关 的 ROS 包 来 建立 ROS 和 仿真 
场景 之 间 的 通信 桥梁 。V-REP 是 一 种 跨 平台 的 软件 ， 可 用 于 不 同 的 操作 系统 ， 如 Windows. Mac 
OS 和 Linux。 它 由 Coppelia Robotics GmbH 开发 ， 并 且 随 附 免费 教育 版 和 商业 版 使 用 许可 。 可 以 
从 Coppelia Robotics 公司 的 下 载 网 址 ， 下 载 最 新 版 本 的 REP， 选 择 Linux 版 本 的 V-REP PRO 
EDU 软件 。 

下 载 网 址 : http://www.coppeliarobotics.com/downloads。 

本 书 使 用 V-REP 的 3.6.2 版 本 。Coppelia 公司 还 提供 其 他 版 本 的 V-REP, 截至 本 书 完稿 ， 最 
新 版 为 4.0.0， 大 家 可 以 自行 选择 。 

1. 下 载 V-REP 

首先 ， 需 要 下 载 V-REP， 这 里 提供 两 种 方式 。 

第 一 种 ， 可 以 在 任何 文件 夹 中 使 用 以 下 命令 下 载 此 版 本 : 


$ wget https://www.coppeliarobotics.com/files/V-REP. PRO.EDU. V3. 6 2 Ubuntui6 04.tar.xz 


第 二 种 ， 可 以 去 官网 下 载 ， 选择 Linux 版 本 ， 教育 版 ， 如 图 5.2 所 示 。 
porheoeoocs ome | 
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选择 V-REP 3.6.2 的 Ubuntu 16.04 版 本 ， 并 选择 教育 版 ， 如 图 $.3 所 示 。 
2. 解压 安装 
下 载 完 成 后 ， 提 取 存 档 : 


$ tar -vxf V-REP. PRO. EDU V3. 6. 2 Ubuntui6 04.tar.xz 


为 了 方便 访问 V-REP 资源 ， 可 以 设置 一 个 环境 变量 CV-REP ROOT) 指向 V-REP 的 主 文 
件 夹 : 
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$ echo 'export V-REP_ROOT="/HOME/V-REP_PRO_EDU_V3_6_2_Ubuntui6_04"' >> ^/.bashrc # 
it’ s same to V-REP3.6.2 


$ source ^/.bashrc 


3.6.2 versions | V-REP 3.6.1 versions 


‘LAYER, Windows V-REP PLAYER, Windows 

'RO EDU, Windows V-REP PRO EDU, Windows 
'RO, Windows V-REP PRO, Windows 

'LAYER, Mac V-REP PLAYER, Mac 

'RO EDU, Mac V-REP PRO EDU, Mac 

'RO, Mac V-REP PRO, Mac 

'LAYER, Ubuntu 16.04 V-REP PLAYER, Ubuntu 16.04 
'RO EDU, Ubuntu 16.04 V-REP PRO EDU, Uburrtu 16.04 
'RO, Ubuntu 16.04 V-REP PRO, Ubuntu 16.04 
‘LAYER, Ubuntu 18.04 V-REP PLAYER, Ubuntu 18.04 
'RO EDU, Ubuntu 18.04 V-REP PRO EDU, Ubuntu 18,04 


图 5.3 Ubuntu 16.04 版 本 下 载 界面 


3. 启动 
回 到 安装 文件 ， 进 入 文件 夹 ， 输 入 以 下 指令 : 


$ ./V-REP.sh 


V-REP 启动 ， 如 图 5.4 所 示 ， 没 出 现 问题 ， 并 且 记 录 log 如 下 。 


Loading the V-REP library... 


Done! 


Launching V-REP... 


V-REP PRO EDU V3.6.2. (rev. 0) 

Using the default Lua library. 

Loaded the video compression library. 

Add-on script 'V-REPAddOnScript-addOnScriptDemo.lua' was loaded. 
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Add-on script 'V-REPAddOnScript-bORemoteApiServer.lua' was loaded. 

Add-on script 'V-REPAddOnScript_PyRep.lua' was loaded. 

If V-REP crashes now, try to install libgli-mesa-dev on your system: 

>sudo apt install libgli-mesa-dev 

OpenGL: VMware, Inc., Renderer: SVGA3D; build: RELEASE; LLVM;, Version: 2.1 Mesa 
18.0.5 

...did not crash. 

Simulator launched. 

Plugin 'MeshCalc': loading... 

Plugin 'MeshCalc': load succeeded. 

Plugin 'Assimp': loading... 

Plugin 'Assimp': warning: replaced variable 'simAssimp' 

Plugin 'Assimp': load succeeded. 

Plugin 'BlueZero': loading... 

Plugin 'BlueZero': warning: replaced variable 'simBO' 


Plugin 'BlueZero': load succeeded. 


Plugin 'BubbleRob': loading... 

Plugin 'BubbleRob': load succeeded. 

Plugin 'Bwf': loading... 

Plugin 'Bwf': load succeeded. 

Plugin 'CodeEditor': loading... 

Plugin 'CodeEditor': load succeeded. 

Plugin 'Collada': loading... 

Plugin 'Collada': load succeeded. 

Plugin 'ConvexDecompose': loading... 

Plugin 'ConvexDecompose': load succeeded. 

Plugin 'CustomUI': loading... 

Plugin 'CustomUI': warning: replaced variable 'simUI' 

Plugin 'CustomUI': warning: replaced function 'simUI.insertTableRow@CustomUL' 
Plugin 'CustomUI': warning: replaced function 'simUI.removeTableRow@CustomUL' 
Plugin 'CustomUI': warning: replaced function 'simUI.insertTableColumnOCustomUT ' 
Plugin 'CustomUI': warning: replaced function 'simUI.removeTableColumnQCustomUI ' 
Plugin 'CustomUI': warning: replaced function 'simUI.setScene3DNodeParamQCustomUI ' 
Plugin 'CustomUI': load succeeded. 


Plugin 'DynamicsBullet-2-78': loading... 
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Plugin 'DynamicsBullet-2-78': load succeeded. 
Plugin 'DynamicsBullet-2-83': loading... 
Plugin 'DynamicsBullet-2-83': load succeeded. 
Plugin 'DynamicsNewton': loading... 

Plugin 'DynamicsNewton': load succeeded. 


Plugin 'DynamicsOde': loading... 

Plugin 'DynamicsOde': load succeeded. 

Plugin 'DynamicsVortex': loading... 

Plugin 'DynamicsVortex': load succeeded. 

Plugin 'ExternalRenderer': loading... 

Plugin 'ExternalRenderer': load succeeded. 

Plugin 'ICP': loading... 

Plugin 'ICP': warning: replaced variable 'simICP' 

Plugin 'ICP': load succeeded. 

Plugin 'Image': loading... 

Error with plugin 'Image': load failed (could not load). The plugin probably couldn't 
load dependency libraries. For additional infos, modify the script 
'libLoadErrorCheck.sh', run it and inspect the output. 

Plugin 'K3': loading... 

Plugin 'K3': load succeeded. 

Plugin 'LuaCommander': loading... 

Plugin 'LuaCommander': warning: replaced variable 'simLuaCmd' 

Plugin 'LuaCommander': load succeeded. 

Plugin 'LuaRemoteApiClient': loading... 

Plugin 'LuaRemoteApiClient': load succeeded. 

Plugin 'Mtb': loading... 

Plugin 'Mtb': load succeeded. 

Plugin 'OMPL': loading... 

Plugin 'OMPL': warning: replaced variable 'simÜMPL' 

Plugin 'OMPL': load succeeded. 

Plugin 'OpenGL3Renderer': loading... 

Plugin 'OpenGL3Renderer': load succeeded. 

Plugin 'OpenMesh': loading... 


Plugin 'OpenMesh': load succeeded. 
Plugin 'Qhull': loading... 
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Plugin 'Qhull': load succeeded. 

Plugin 'RRS1': loading... 

Plugin 'RRS1': load succeeded. 

Plugin 'ReflexxesTypeII': loading... 

Plugin 'ReflexxesTypeII': load succeeded. 

Plugin 'RemoteApi': loading... 

Starting a remote API server on port 19997 

Plugin 'RemoteApi': load succeeded. 

Plugin 'RosInterface': loading... 

RosInterface: ROS master is not running 

Error with plugin 'RosInterface': load failed (failed initialization). 
Plugin 'SDF': loading... 

Plugin 'SDF': warning: replaced variable 'simSDF' 

Plugin 'SDF': load succeeded. 

Plugin 'SurfaceReconstruction': loading... 

Plugin 'SurfaceReconstruction': warning: replaced variable 'simSurfRec' 
Plugin 'SurfaceReconstruction': load succeeded. 

Plugin 'Urdf': loading... 

Plugin 'Urdf': load succeeded. 

Plugin 'Vision': loading... 


Plugin 'Vision': load succeeded. 


Using the 'MeshCalc' plugin. 


DL 


图 5.4 V-REP 启动 界面 
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4. 与 外 部 应 用 通信 

V-REP 提供 以 下 模式 用 于 从 外 部 应 用 控制 仿真 机 器 人 。 

Remote API: V-REP 远程 API 由 若干 函数 组 成 ， 这 些 函 数 可 以 由 C/C++. Python. Lua 或 者 
MATLAB 开发 的 外 部 应 用 调用 。 远程 API 5j V-REP 交互 使 用 套 接 字 通信 。 为 了 将 ROS 和 仿真 场 
景 连接 起 来 ， 可 以 将 V-REP 远程 API 集成 到 C++ 或 者 Python 节点 中 。 可 以 在 Coppelia Robotics 
网 站 (http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctions.htm) 上 找 
到 V-REP 中 所 有 可 调用 的 远程 API。 要 使 用 远程 API， 就 必须 同时 准备 好 客户 端 和 服务 器 端 。 

* V-REP 客户 端 : 位 于 外 部 应 用 中 。 它 可 在 ROS 节点 中 实现 ， 也 可 在 由 V-REP 支持 的 编 

程 语音 所 编写 的 标准 程序 中 实现 。 
* V-REP 服务 器 端 : 由 V-REP 脚本 实现 。 它 允许 仿真 器 接收 外 部 的 数据 来 与 仿真 场景 进行 
XH. 

RosPlugin: V-REP 的 ROS 插件 实现 了 一 个 高 级 抽象 ， 可 直接 将 仿真 物体 与 场景 和 ROS 3 
信 系 统 连接 起 来 。 使 用 此 插件 ， 可 以 自动 使 用 订阅 的 消息 ， 并 发 布 来 自 场景 物体 的 话题 ， 从 而 
获取 信息 或 控制 仿真 机 器 人 。 

RosInterface: 最 新 版 的 V-REP 中 引入 了 RosInterface， 在 以 后 的 版 本 中 该 接口 将 替换 
RosPlugin. 5j RosPlugin 不 同 ， 该 模块 复制 C++ API 函数 ， 从 而 允许 ROS 45 V-REP 通信 。 

RosInterface 是 V-REP 官方 推荐 的 用 来 跟 ROS 通信 的 插件 。 本 书 中 ， 我 们 将 讨论 如 何 使 用 
RosInterface 与 V-REP 进行 交互 。 

对 于 不 同 版 本 的 V-REP, 启用 RosInterface 有 一 定 区 别 , 但 是 基本 一 致 。 对 于 低 版 本 , 可 能 需 
要 使 用 者 从 源 代码 进行 编译 ， 生 成 .so 文件 。 对 于 高 版 本 使 用 者 需要 从 安装 文件 下 compiledRos 
中 的 .so 文件 复制 到 根 目录 下 。 本 书 选用 的 3.6.2 版 本 ， 此 .so 文件 已 放置 在 根 目录 下 ,无须 任 何 
操作 ， 如 图 5.5 所 示 。 


> sh, 以 
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| rosinterf.btm rostutoriathtm  rosinterfoces.htm — rosinterfaceApi. 
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5.5 Roslnterface 文件 


122 HAR ASSO 


至 此 ，V-REP 的 安装 就 告 一 段落 了 ， 下 面 让 我 们 开始 使 用 V-REP 吧 。 


5.1.3  V-REP 的 简单 使 用 


前 面 , 我 们 讲述 了 如 何 下 载 和 安装 V-REP 计算 机 。 本 节 中 , 我们 来 熟悉 软件 的 基本 使 用 方法 。 
1. 软件 启动 界面 
软件 启动 界面 如 图 5.6 所 示 。 


5.6 ”软件 启动 界面 


与 大 多 数 开 发 软件 一 样 ，V-REP 软件 的 主 界 面 分 为 以 下 几 部 分 : 

Al: 顶部 菜单 栏 和 工具 栏 。 其 中 ， 工 具 栏 中 是 一 些 对 环境 和 模型 的 基本 操作 过 程 ， 包 括 视 
图 转换 ， 视 图 放大 缩小 ， 机 器 人 模型 的 平移 、 旋 转 等 ， 以 及 物理 引擎 的 选择 ， 仿 真 程序 的 启动 、 
暂停 和 加 减速 等 基本 功能 。 

A2: 侧 边栏 工具 栏 用 于 展开 /隐藏 模型 文件 树 和 场景 文件 树 (Scene)。Scene 是 一 个 工程 环 
境 ， 新 建 Scene 可 新 建 仿真 环境 ， 在 里 面 添加 所 需 的 模型 即 可 。 

B: 模型 文件 树 。 用 于 管理 和 展示 软件 自 带 的 各 种 模型 以 及 自 定义 的 模型 ,里 面包 括 了 各 种 
现成 的 机 器 人 模型 和 桌子 、 沙 发 、 门 等 基本 的 模型 组 件 ， 组 合 这 些 组 件 可 以 构造 所 需要 的 仿真 
环境 。 

C: 场景 文件 树 (Scene)。 每 次 执行 一 个 仿真 的 任务 时 ， 所 有 的 模型 组 件 都 被 放置 到 这 个 
文件 目录 下 ， 在 新 建 一 个 Scene 时 ， 软 件 会 自 带 一 个 Camera 和 3 个 基本 模型 ，Floor、Light 和 


Camera. 
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* Floor 是 一 个 放置 模型 的 “地 板 ”。 因 为 V-REP 是 一 个 物理 仿真 环境 所 有 有 质量 的 物体 
在 该 环境 中 都 会 受到 重力 的 作用 ， 如 果 没 有 Floor， 模 型 束 会 “ 掉 下 去 ”。 大 家 可 以 试 着 
拖 动 一 个 模型 到 Floor 以 外 的 区 域 ， 然 后 单 击 “ 开 始 ” 按 钮 试 一 试 。 

* Light 是 光源 。 在 三 维 场景 中 ， 如 果 没 有 光源 ， 就 无 法 看 见 物体 ， 也 没有 立体 的 感觉 ， 所 
以 需要 光源 。 更 重要 的 ， 如 果 要 在 仿真 环境 中 使 用 光学 类 传感器 〈 如 Camera)， 没 有 光 
源 是 无 法 使 用 的 。 用 户 可 以 根据 需要 自行 添加 Light， 包 括 点 状 的 、 线 性 的 光源 等 。 

* Camera 是 摄像 机 。 用 于 在 机 器 人 仿真 时 ， 从 各 个 角度 来 观察 机 器 人 的 运动 仿真 情况 ， 默 
认 的 Camera 包含 了 从 Ozyz 坐标 系 的 各 个 平面 进行 观察 的 Camera， 也 可 以 同时 进行 多 
个 窗口 的 观察 ， 这 一 点 在 后 文 会 看 到 。 

D: 仿真 环境 的 可 视 化。 我 们 所 有 的 模型 在 实际 仿真 过 程 中 的 运动 都 通过 该 区 域 的 可 视 化 来 
展示 ， 便 于 直观 地 观察 。 当 然 ， 演 染 复 杂 的 三 维 模型 需要 的 计算 力 比较 多 ， 当 模型 比较 复杂 时 
可 以 通过 关闭 可 视 化 来 提升 仿真 的 速度 。 

E: 状态 栏 。 用 于 显示 当前 仿真 环境 的 状态 。 

2. 一 些 基础 操作 

软件 的 工具 栏 可 以 根据 自己 的 需求 拖 动 放 在 任意 位 置 。 

在 软件 的 场景 文件 树 区 域 ， 右 击 可 以 选择 添加 Light、Joint 等 各 种 组 件 到 当前 的 Scene， 如 
图 5.7 所 示 。 

在 软件 的 可 视 化 区 域 ， 右 击 可 以 选择 可 视 化 的 模式 ， 例 如 是 否 显示 网 格 线 等 ， 如 图 5.8 
所 示 。 
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5.8 ”可 视 化 选择 


3. Scene 介绍 

Scene 是 一 个 基本 的 工程 环境 ， 相 当 于 一 个 容器 ， 在 这 个 容器 里 面 放置 各 种 模型 以 进行 仿 
真 。 新 建 、 保 存 和 关闭 当前 Scene 在 菜单 栏 可 以 找到 ， 如 图 5.9 Prax. Scene 文件 的 扩展 名 为 .ttt， 
名 字 可 以 自行 设 定 , 保存 位 置 也 可 以 自 定义 。 所 有 加 入 仿真 环境 的 模型 和 传感器 等 都 会 在 Scene 
中 展现 ， 所 以 在 实际 仿真 操作 过 程 中 ，Scene 是 一 个 基本 操作 台 。 


图 5.9 Scene 工程 环境 


4. 使 用 现 有 的 模型 
我 们 使 用 现 有 的 机 器 人 模型 来 熟悉 软件 的 操作 过 程 。 在 新 建 好 Scene 以 后 ， 从 左边 的 模型 
文件 树 里 面 选择 要 仿真 的 模型 ， 直 接 用 鼠标 左 键 选中 并 拖 入 右边 的 可 视 化 区 域 即 可 ， 这 里 我 们 
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选择 了 一 个 移动 式 的 多 是 爬行 机 器 人 路径: robots/mobile/ant hexapod.ttm) 和 一 个 非 移 动 式 的 
HARES Crobots/non-mobile/ABB IRB 4600-40-255.ttm)。 

我 们 可 以 通过 工具 栏 的 平移 、 旋 转 、 缩 放 等 按钮 来 转移 可 视 化 区 域 的 视角 ， 便 于 完整 地 观 
察 机 器 人 的 模型 ， 如 图 5.10 所 示 。 


@ now 
图 5.10 转移 可 视 化 区 域 的 视角 


放置 好 机 器 人 模型 以 后 ， 如 果 觉 得 机 器 人 放置 的 位 置 不 合适 ， 可 以 通过 工具 栏 的 模型 平移 、 
旋转 等 操作 来 移动 机 器 人 模型 ， 以 确保 其 处 于 一 个 比较 理想 的 位 置 和 姿态 ， 如 图 5.11 所 示 。 


archy 2 
cene (scene 1) Ts 


图 5.11 调整 姿态 


一 切 就 绪 以 后 ， 我 们 可 以 发 现 ， 在 Scene 中 多 出 2 个 模型 文件 ， 它 们 分 别 对 应 我 们 刚刚 加 
入 的 2 个 模型 ， 如 图 5.12 所 示 。 


5.12 ABB fil ANT 
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单 击 模型 文件 左边 的 加 号 ， 可 以 展开 模型 文件 。 单 击 模型 文件 中 对 应 的 部 分 ，V-REP 软件 
会 指示 我 们 该 部 分 文件 对 应 模型 的 哪 一 部 分 结构 ， 如 图 5.13 和 图 5.14 所 示 。 
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图 5.14 机 器 人 模型 


此 外 ， 双 击 Scene 文件 树 中 对 应 模型 名 称 后 面 的 那个 文件 符号 ， 可 以 获取 到 该 模型 的 控制 
代码 ， 其 使 用 Lua 语言 编写 ， 如 果 想 要 使 用 Lua 语言 编写 自己 的 控制 代码 ， 那 么 就 可 以 在 这 里 
编写 。 这 次 我 们 先 使 用 模型 自 带 的 代码 进行 一 个 Demo 运行 展示 。 图 5.15 所 示 为 Lua 程序 。 


Sas as 


5.5 Lua 程序 


5. 仿真 过 程 

通过 前 面 的 步 又， 我 们 已 经 新 建 好 了 自己 的 仿真 环境 ， 加 载 了 对 应 的 模型 。 那 么 ， 接 下 来 
就 可 以 利用 模型 自 带 的 控制 代码 进行 仿真 了 。 首 先 ， 我 们 来 熟悉 一 下 工具 栏 中 与 仿真 相关 的 部 
分 。 图 5.16 为 仿真 界面 。 
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O 物理 引擎 的 选择 。V-REP 自 带 Bullet, ODE, Newton 等 几 种 基本 的 物理 引擎 ， 用 户 可 以 
自行 选择 ， 对 于 初学 者 来 说 ， 这 几 个 物理 引擎 的 差别 不 大 ， 默 认 即 可 。 

@ 仿真 精度 。 由 于 高 精度 的 仿真 计算 和 跟踪 过 程 需要 消耗 大 量 的 计算 力 ， 因 此 用 户 可 以 根 
据 自己 的 需求 选择 合适 的 精度 ， 初 学 者 默认 即 可 。 

@ 仿真 周期 。 即 仿真 循环 的 时 间 周 期 ， 不 同 的 时 间 对 应 不 同 的 仿真 速度 ， 初 学 者 默认 即 可 。 

@ 开始 /暂停 /停止 。 

@ 加 速 /减速 仿真 过 程 。 

@ 多 窗口 观测 仿真 过 程 。 

单 击 仿真 后 ， 可 以 看 到 机 械 臂 及 机 器 人 开始 运动 。 图 5.17 所 示 为 仿真 效果 。 

到 此 为 止 ，V-REP 软件 的 基本 使 用 过 程 已 经 介绍 完了 ， 更 多 关于 V-REP 的 使 用 说 明 ， 读 者 
可 以 通过 以 下 网 址 查看 V-REP 的 官方 手册 : 
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https://www.coppeliarobotics.com/helpFiles/index.html 
下 面 我 们 将 开始 理解 V-REP_RosInterface 插件 的 使 用 。 
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图 5.17 仿真 效果 

5.1.4 ”理解 RosInterface 

RosInterface 是 V-REP API 框架 的 一 部 分 ， 由 Federico Ferri 提供 。 确 保 不 要 将 RosInterface 
与 RosPlugin 混淆 ， 后 者 是 V-REP 已 弃 用 的 接口 。RosInterface 具有 很 好 的 保 真 度 ， 它 复制 了 
C/C++ ROS API。 这 使 其 成 为 通过 ROS 进行 灵活 通信 的 理想 选择 ， 但 可 能 需要 更 多 了 解 各 种 消 
‘All ROS 的 运行 方式 。 

V-REP 可 以 充当 ROS 节点 ， 其 他 节点 可 以 通过 ROS 服务 、ROS 发 布 者 和 ROS 订阅 者 与 之 
进行 通信 。 

V-REP 中 的 RosInterface 功能 通过 以 下 插件 启用 : libv repExtRosInterface.so 或 libv repExt- 
RosInterface.dylib。 插 件 的 代码 可 以 在 这 里 找到 ; 

https://github.com/CoppeliaRobotics/v_repExtRosInterface 

该 插件 可 以 轻松 地 适应 您 自己 的 需求 。 启 动 V-REP 时 会 加 载 该 插件 , 但 是 只 有 当 roscore IE 
在 运行 时 ， 加 载 操 作 才 会 成 功 。 确 保 检查 V-REP 的 控制 台 窗 口 或 终端 以 获取 有 关 插 件 加 载 操 作 
的 详细 信息 。 

如 果 仿 真 场景 需要 RosInterface， 但 由 于 roscore 没有 在 运行 仿真 器 之 前 运行 ， 或 者 roscore 
没有 安装 到 系统 上 ， 就 会 弹出 一 个 错误 窗口 来 提示 用 户 ， 如 图 5.18 所 示 。 
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没有 运行 roscore 的 情况 下 使 用 RosInterface 时 会 显示 错误 


如 果 正 确 配置 了 V-REP， 启 动 ROS， 再 启动 V-REP， 即 可 查看 到 RosInterface 已 启动 ， 操 作 
指令 如 下 : 


$ roscore 
$ ./V-REP.sh 


$ rosnode list 


如 图 5.19 所 示 ， 功 能 正常 。 
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图 5.19 Interface 节点 
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正确 加 载 RosInterface 并 启动 V-REP 后 , V-REP 将 以 一 个 名 称 为 /V-REP_ros_interface 的 ROS 
节点 方式 运行 。 根 据 官方 提供 的 案例 ， 碍 看 以 下 模拟 场景 模型 ， 可 以 快速 开始 使 用 RosInterface。 

1. non threaded 

例子 : 在 空 的 V-REP 场景 中 选择 一 个 对 象 ， 然 后 使 用 Menu bar — Add — Associated child 
script — non threaded 命令 将 非 线程 的 子 脚本 附加 到 该 对 象 。 打 开 该 脚本 的 脚本 编辑 器 ， 然 后 将 
内 容 替 换 为 以 下 内 容 : 


function subscriber. callback(msg) 


-- This is the subscriber callback function 
sim.addStatusbarMessage('subscriber receiver following Float32: '..msg.data) 


end 


function getTransformStamped(objHandle,name,relTo,relToName) 
-- This function retrieves the stamped transform for a specific object 
t-sim.getSystemTime() 
p=sim. getObjectPosition(objHandle,relTo) 
o-sim.getÜbjectQuaternion(objHandle,relTo) 
return { 
header={ 
stamp=t, 
frame_id=relToName 
E 
child_frame_id=name, 
transform={ 
translation={x=p[1] ,y=p[2] ,z=p[3]}, 
rotation-(x-o[1],y-0[2] ,z=o[3] , w=o[4]} 


end 


function sysCall, init() 
-- The child script initialization 
objectHandle-sim.getÜbjectAssociatedWithScript (sim. handle_self) 
objectName=sim. getObjectName(objectHandle) 
-- Check if the required RosInterface is there: 


moduleName=0 
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index=0 
rosInterfacePresent=false 
while moduleName do 
moduleName=sim. getModuleName (index) 
if (moduleName=='RosInterface') then 
rosInterfacePresent=true 
end 
index=index+1 


end 


-- Prepare the float32 publisher and subscriber (we subscribe to the topic we 
advertise): 
if rosInterfacePresent then 
publisher=simR0S.advertise('/simulationTime', 'std_msgs/Float32') 
subscriber=simROS.subscribe('/simulationTime' , 'std_msgs/Float32', 'subscriber_ 
callback') 
end 


end 


function sysCall actuation() 
-- Send an updated simulation time message, and send the transform of the object 
attached to this script: 

if rosInterfacePresent then 
simROS.publish(publisher, {data=sim. getSimulationTime()}) 
simROS.sendTransform(getTransformStamped(objectHandle, objectName,-1, 'world')) 
-- To send several transforms at once, use simROS.sendTransforms instead 

end 


end 


function sysCall, cleanup() 
-- Following not really needed in a simulation script (i.e. automatically shut down 
at simulation end): 
if rosInterfacePresent then 


simROS.shutdownPublisher (publisher) 


132 4 仿 人 机 器 人 建 模 与 控制 


simROS. shutdownSubscriber (subscriber) 


end 


end 


上 面 的 脚本 将 发 布 模拟 时 间 , 并 同时 订阅 它 。 它 还 将 发 布 脚本 附加 到 对 象 的 转换 , 如 图 5.20 
所 示 。 
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图 5.20 non threaded 


在 终端 输入 如 下 指令 ， 碍 看 话题 ， 见 图 5.21。 


$ rostopic list 


5.21 rostopic 


为 了 查看 消息 的 内 容 ， 可 以 输入 : 


$ rostopic echo /simulationTime 
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消息 内 容 如 图 5.22 所 示 。 


图 5.22 simulationTime 


脚本 中 主要 用 到 下 面 几 个 函数 。 
simExtRosInterface_advertise， 如 图 5.23 Aras. 


Description Advertise a topic and create a topic pubisher. 

Lua synopsis int publsherHandie=simExtRosintertace_advertise(string topicName, string topicType, int queueSize = 1, 
bool latch = false) 

Lua parameters topicName: topic name, e.g.: '/cmd vel 


topicType: topic type, e.g.: ‘geometry_msgs:: Twist’ 
queuesSize: (optional) queue size 
latch: (optional) latch topic 

Lua return values publisherHandle: a handle to the ROS publisher 


5.23 V-REP #2 Padvertise 


simExtRosInterface subscribe, in B| 5.24 所 示 。 


Description Subscribe to a topic. 

Lua synopsis int subscriberHandle-simExtRosInterface subscribe(string topicName, string topicType, string 
topicCalback, int queueSize = 1) 

Lua parameters topicName: topic name, e.g.: '/cmd. vel 


topicType: topic type, e.g.: 'geometry msgs::Twist" 
topicCallback: name of the callback function, which will be called with a single argument of type table 
containing the message payload, e.g.: (linear (x- 1.5, y=0,0, z=0.0}, angular={x=0.0, y=0.0, z=-2,3}} 
queueSize: (optional) queue size 

Lua return values — subscriberHandle: a handle to the ROS subscriber 
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simExtRosInterface publish, WE 5.25 所 示 。 


ace 


(Lua synopsis —— s 
‘Lua parameters 


plein eae a 


[Lua return values = 


5.25 publish 


simExtRosInterface sendTransform, "JE 5.26 所 示 。 


5.26 sendTransform 


2. rosInterfaceTopicPublisherAndSubscriber 

在 V-REP 自 带 的 例子 中 ， 还 有 一 个 场景 模型 rosInterfaceTopicPublisherAndSubscriberttt。 在 
此 场景 中 ， 附 加 到 Vision. sensor 的 子 脚本 中 的 代码 将 使 发 布 者 能 够 流 式 传输 视觉 传感器 的 图 像 ， 
还 使 订阅 者 能 够 收听 相同 的 流 。 订 阅 者 将 读 取 的 数据 应 用 于 被 动 视觉 传感器 时 ， 该 被 动 视觉 伟 
感 器 仅 用 作 数据 容 器 ， 即 将 视觉 传感器 捕获 的 图 像 信息 发 布 到 /image 话题 上 ， 同 时 会 自己 订阅 
这 个 信息 并 显示 出 来 。 图 像 发 布 与 订阅 案例 如 图 5.27 Pras. 


Subscriber reads and applies the image here 


5.27 图 像 发 布 与 订阅 案例 


尝试 一 下 代码 。 可 以 使 用 以 下 命令 可 视 化 V-REP 流 式 传输 的 图 像 ， 机 器 人 摄像 头 数据 流 示 
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意图 。 如 图 5.28 所 示 。 


$ rosrun image_view image_view image:=/visionSensorData 


528 机 器 人 摄像 头 数据 流 示意 图 


如 果 想 传输 更 简单 的 数据 ， 还 可 以 通过 以 下 方式 将 其 可 视 化 : 


$ rostopic echo /visionSensorData 


如 图 5.29 所 示 ， 在 终端 中 可 以 显示 出 机 器 人 图 像 传感器 中 获取 到 的 原始 图 像 数据 。 


-— ex 


5.29 图 像 传感器 获取 的 原始 图 像 数据 


3. controlTypeExamples 


还 有 一 个 例子 是 controlTypeExamples.ttt，V-REP 中 的 脚本 负责 发 布 接近 传感器 的 信息 以 及 
仿真 时 间 并 订阅 左右 轮 驱 动 的 话题 。 外 部 的 ROS 程序 rosBubbleRob2 根据 接收 到 的 传感器 信息 
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iat TA) 


生成 左右 轮 速度 指令 ， 并 发 布 出 去 ，V-REP 中 订阅 后 在 回调 函数 里 控制 左右 轮 关节 转动 。 
外 部 客户 端 应 用 程序 通过 ROS 控制 机 器 人 如 图 5.30 所 示 。 


Ý V-REP PRO - controlledViaScript - rendering: 3 ms (7.9 fps) SIMULATION STOPPED - a x 


Cigfeuhzamens 
h Tertullian, 

ghost 

Sieh i 


图 5.30 外 部 客户 端 应 用 程序 通过 ROS 控制 机 器 人 


在 模型 浏览 器 的 tools 文件 夹 中 有 一 个 RosInterface 的 帮助 工具 ， 如 图 5.31 所 示 ， 将 其 拖 入 
场景 中 可 以 方便 实现 一 些 控制 功能 。 


= (&. DafedlllightA 
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图 5.31 帮助 工具 
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主要 有 下 面 一 些 功能 : 

* startSimulation topic: 可 以 通过 发 送 一 个 std_msgs::Bool 类 型 消息 ， 启 动 仿真 。 

* pauseSimulation topic: 可 以 通过 发 送 一 个 std_msgs::Bool 类 型 消息 ， 暂 停 仿真 。 

* stopSimulation topic: 可 以 通过 发 送 一 个 std_msgs::Bool 类 型 消息 ， 停 止 仿真 。 

* enableSyncMode topic: 可 以 通过 发 送 一 个 std_msgs::Bool 类 型 消息 ， 开 关 同 步 传真 。 

* triggerNextStep topic: 可 通过 发 送 一 个 std_msgs::Bool 类 型 消息 ， 触 发 下 一 个 仿真 。 

* simulationStepDone topic: 一 个 std_msgs::Bool 类 型 消息 会 在 每 个 仿真 结束 时 被 发 送出 来 。 

* simulationState topic: 定期 发 送 std_msgs::Int32 类 型 消息 , 0 代表 仿真 停止 1 代表 仿真 正 
在 允许 ，2 代表 仿真 被 暂停 了 。 

* simulationTime topic: 定期 发 送 std_msgs::Float32 类 型 消息 ， 显 示 当 前 的 模拟 时 间 。 

可 以 在 终端 中 输入 下 面 的 一 些 命令 进行 测试 。 


$ rostopic pub /startSimulation std msgs/Bool true --once 
$ rostopic pub /pauseSimulation std_msgs/Bool true --once 
$ rostopic pub /stopSimulation std_msgs/Bool true --once 
$ rostopic pub /enableSyncMode std_msgs/Bool true --once 
$ rostopic pub /triggerNextStep std_msgs/Bool true --once 


比如 在 终端 中 输入 rostopic pub /startSimulation std_msgs/Bool true -once， 就 可 以 开始 仿真 ， 
跟 手 动 单 击 仿真 按钮 效果 一 样 ， 如 图 5.32 所 示 。 


图 5.32 Bool true 


至 此 ，V-REP 与 ROS 的 通信 原理 已 经 基本 了 解 ， 下 面 开始 导入 Roban 机 器 人 进行 实际 的 
使 用 。 
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5.2  V-REP 中 的 Roban 机 器 人 


关于 V-REP 的 基本 使 用 ， 在 5.1 节 已 经 进行 了 一 定 的 学 习 ， 下 面 我 们 将 基于 Roban 机 器 人 
完成 一 些 DEMO， 其 中 涉及 V-REP 环境 中 的 机 器 人 感知 和 控制 过 程 ， 包 括 机 器 人 视觉 的 感知 、 
机 器 人 运动 规划 和 控制 、 机 器 人 路 径 规划 和 控制 等 。 


5.2.1 SA Roban 机 器 人 


首先 ， 根 据 前 几 章 的 学 习 ， 我 们 知道 Roban 机 器 人 是 通过 ROS 进行 操作 的 ， 那 么 ， 我 们 需 
要 把 乐 聚 官方 提供 的 Roban-V-REP 资源 包 放 入 ROS 的 工作 空间 ， 然 后 进行 编译 。 

关于 资源 包 , 读者 可 以 通过 以 下 网 址 进行 获取 , 将 资源 包 git clone 下 载 到 本 地 。 乐 聚 github 
网 址 : 

https://github.com/LejuRobotics 

获得 资源 包 之 后 ， 将 其 放 入 工作 空间 ， 根 据 第 3 章 内 容 ， 这 里 我 们 新 建 一 个 工作 空间 ， 通 
过 如 下 指令 : 


$ mkdir -p ^/rosV-REP/src 
$ cd ~/rosV-REP/ 


将 资源 包 放 入 src 文件 夹 之 后 ， 进 行 编译 : 


E catkin_make 


第 一 次 编译 会 有 点 儿 慢 ， 读 者 不 要 着 急 。 编 译 完成 后 ， 效 果 如 图 5.33 所 示 。 

在 正常 情况 下 ， 都 是 可 以 编译 成 功 的 ， 但 是 部 分 读者 可 能 会 编译 失败 ， 这 可 能 是 由 ROS 的 
某 些 依赖 包 没有 被 安装 造成 的 ， 读 者 可 以 通过 查看 报错 上 日志， 对 缺失 的 包 进 行 下载 ， 放 入 工作 
空间 后 ， 再 进行 编译 。 这 里 可 能 使 用 到 的 依赖 包 ， 在 官方 提供 的 资源 包 中 已 内 置 ， 遇 到 具体 情 
况 可 查阅 官方 论坛 。 

编译 成 功 后 , 就 可 以 开始 关于 Roban-V-REP 的 学 习 。 读 者 可 以 看 到 sre 文件 夹 下 有 很 多 功能 
包 ， 它 们 涉及 Roban 机 器 人 的 运动 、 步 态 、 传 感 器 等 多 个 方面 ， 本 节 仅 对 BodyHub 进行 初步 的 
展开 ， 引 导读 者 如 何在 V-REP 中 导入 Roban 机 器 人 。 后 续 我 们 将 详细 介绍 Roban 的 虚拟 仿真 。 

首先 ， 打 开 终端 ， 启 用 ROS: 


$ roscore 


图 5.34 所 示 为 roscore 启动 。 
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图 5.33 编译 成 功 


ore ht /venus-virtual-machl 


started roslaunch server http: //venus-virtual-machine:43729/ 


ros comm ve ion 1.12.14 


aut 


iprocess[naster started with pid [2887] 
ROS MASTER URI-http://venus-virtual-machiíne:11311/ 


setting /run id to 55a22d22-9050-11e23-9e97-08Bedb9c6d2b5 
started with pid [2900] 


5.34 roscore 启动 


然后 ， 打 开 V-REP 仿真 软件 ， 并 用 软件 打开 */bodyhub/V-REP/Roban.ttt 场景 文件 。 
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$ ./V-REP.sh 


图 5.35 所 示 为 V-REP 导入 Roban 机 器 人 。 


5.35 V-REP 导入 Roban 机 器 人 


至 此 ，Roban 机 器 人 就 已 经 导入 VREP 中 了 。 读 者 想 要 控制 Roban 模型 ， 需 要 结合 第 3 章 
ROS 的 相关 知识 。 


5.2.2 BodyHub 简介 与 启动 


通过 前 面 的 学 习 和 操作 , 读者 肯定 发 现 了 , 我 们 编译 了 BodyHub 的 包 , 并 从 中 导入 了 Roban 
模型 ， 那 么 ，BodyHub 包 有 具体 有 什么 作用 了 呢 ? 

BodyHub 是 一 个 节点 , 是 上 位 机 其 他 节点 与 下 位 机 通信 的 中 间 节 点 , 机 器 人 的 所 有 控制 指令 
和 数据 获取 都 通过 此 节点 实现 。 图 5.36 即 为 V-Rep 和 BodyHub 包 正 确 启动 后 的 情况 。BodyHub 
实现 了 一 个 状态 机 ， 用 BodyHub 统一 管理 下 位 机 可 确保 如 舵 机 这 类 设备 在 同一 时 刻 只 有 一 个 上 
层 节 点 控制 ， 吉 免 节点 之 间 的 干扰 导致 控制 异常 。 这 一 节 ， 我 们 先 做 一 个 简单 介绍 ， 后 续 我 们 
将 对 其 进行 深入 学 习 。 另 外 ， 读 者 可 以 阅读 BodyHub 包 下 的 说 明文 件 以 对 其 有 进一步 的 了 解 。 

在 学 习 BodyHub 节点 之 前 ， 我 们 先 了 解 一 下 状态 机 。 状 态 机 能 够 根据 控制 信号 按照 预先 设 
定 的 状态 进行 状态 转移 ， 是 协调 相关 信和 号 动作 , 完成 特定 操作 的 控制 中 心 。 通 俗 地 说 ， 就 是 状态 
转移 图 ， 节 点 在 这 个 图 状态 之 间 转 换 。 


第 $ 章 “V=REP 使 用 概述 lp 141 


Æ 5.36 BodyHub 节点 


BodyHub 实现 了 一 个 自 定义 的 状态 机 ， 用 来 管理 上 层 节点 对 其 部 分 服务 的 请 求 。BodyHub 
节点 有 如 下 功能 。 

(1) 主要 功能 : 根据 上 层 节 点 给 的 数据 控制 机 器 人 舵 机 ， 实 现 机 器 人 运动 控制 。 

(2) 次 要 功能 : 获取 机 器 人 传感器 数据 ， 发 布 数据 ;根据 上 层 指 令 ， 控 制 机 器 人 的 执行 器 。 

BodyHub 节点 的 服务 是 独占 的 ， 同 一 时 刻 只 能 响应 一 个 上 层 节 点 的 请 求 ， 为 避免 BodyHub 
节点 在 处 理 某 个 请 求 时 被 其 他 节点 的 请 求 干扰 ， 我 们 使 用 了 状态 机 进行 管理 。 

以 上 介绍 完了 BodyHub 节点 和 状态 机 , 读者 可 能 还 不 是 很 清楚 它们 具体 的 作用 。 简单 来 说 ， 
我 们 想 要 控制 Roban 机 器 人 ， 首 先 需要 占用 BodyHub 节点 ; 然后 涉及 Roban 机 器 人 的 运动 、 传 
感 器 等 控制 ， 则 要 使 用 状态 机 使 BodyHub 节点 处 于 对 应 状态 。 这 里 我 们 是 给 大 家 一 个 框架 印象 ， 
接 下 来 我 们 将 通过 实际 操作 来 讲解 如 何 控制 Roban 模型 仿真 。 

1. 运行 BodyHub 节点 

在 已 经 运行 roscore 的 情况 下 ， 新 建 一 个 终端 ， 通 过 如 下 指令 ， 进 入 工作 空间 ， 并 将 当前 工 
作 空 间 设置 在 ROS 工作 环境 的 最 顶层 : 


$ cd ~/rosV-REP/ 


$ source devel/setup.bash 


然后 ， 在 该 终端 下 执行 以 下 命令 ， 以 仿真 模式 启动 节点 ， 运 行 结果 参考 图 5.37。 


$ roslaunch bodyhub bodyhub.launch sim:=true 
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Eft ap gw 


- e ANN rv nagd 
eni 


图 5.37 启动 节点 

2. 占用 BodyHub 节点 
BodyHub 节点 启动 成 功 后 ， 我 们 假设 占用 ID 设置 为 6， 即 使 用 ID 为 6 的 节点 占用 状态 机 ， 
向 BodyHub 节点 的 /MediumSize/BodyHub/StateJump 服务 发 送 如 下 请 求 ， 请 求 占用 H: 


p rosservice call /MediumSize/BodyHub/StateJump "masterID: 6 
|> stateReq: 'setStatus'" 


或 


js rosservice call oaa Ga CUN Moon gin 6 setStatus 


HE 回 以 下 内 容 ， 则 占用 成 功 ， 如 图 5.38 所 示 。 


stateRes: 22 


图 5.38 占用 状态 机 


车 返回 其 他 内 容 ， 则 占用 失败 。 
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另外 ， 请 注意 : 47 BodyHub 节点 占用 ID 不 为 0， 则 返回 被 占用 ID; 若 为 0， 设 置 请 求 的 占 
用 ID 并 返回 ready 状态 ， 此 时 BodyHub 节点 被 占用 成 功 s 

3. 使 用 BodyHub 节点 

BodyHub 节点 被 成 功 占用 后 ， 就 可 以 使 用 BodyHub 节点 实现 我 们 功能 所 需 的 一 些 话题 和 服务 。 

1) 订阅 的 话题 

* /MediumSize/BodyHub/MotoPosition(bodyhub::JointControlPoint) 

用 于 所 有 关节 的 控制 。 消 息 定义 : 


float64[] positions 


float64[] velocities 
float64[] accelerations 
float64[] effort 


duration time_from_start 


uinti6 mainControlID 


Jd, mainControlID 为 占用 节点 的 控制 ID. 

* /MediumSize/BodyHub/HeadPosition(bodyhub::JointControlPoint) 
用 于 头 部 关节 的 控制 。 

* /BodyHub/SensorControl(bodyhub::SensorControl) 
用 于 外 接 执行 器 的 控制 。 消 息 定义 : 


string SensorName 
uinti6 SetAddr 
uint8[] ParamList 


其 中 ，SensorName 为 执行 器 名 称 ，SetAddr 为 执行 器 参数 地 址 ，ParamList 为 执行 器 参数 
列表 。 

* /simulationStepDone(std_msgs/Bool) 

表示 仿真 的 一 帧 执行 完成 。 

* /simulationState(std_msgs/Int32) 

表示 仿真 的 状态 。 

* /sim/joint/angle(std_msgs/Float64MultiArray) 

表示 仿真 机 器 人 的 关节 角度 。 

* /sim/joint/velocity(std_msgs/Float64MultiArray) 

表示 仿真 机 器 人 的 关节 速度 。 

* /sim/forceTorque/leftFoot(std_msgs/Float64MultiArray) 
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仿真 机 器 人 的 左 脚 力 传感器 的 数值 。 

* /sim/forceTorque/rightFoot(std_msgs/Float64MultiArray) 
仿真 机 器 人 的 右 脚 旋 传感器 的 数值 。 

2) 发 布 的 话题 

* /MediumSize/BodyHub/Status(std_msgs/UInt16) 

节点 状态 机 的 状态 。 状 态 定义 : 


enum StateStyle { 

init = 20, 
preReady, 
ready, 

running, 

pause, 

stoping, 

error, 


directOperate, 


walking 
Js 


* /MediumSize/BodyHub/ServoPositions(bodyhub::ServoPositionAngle) 
机 器 人 的 关节 角度 。 

* /jointPosTarget(std_msgs/Float64MultiArray) 

机 器 人 关节 的 期 望 角度 。 

* /jointPosMeasure(std_msgs/Float64MultiArray) 

机 器 人 关节 的 实际 角度 。 

* /jointVelTarget(std_msgs/Float64MultiArray) 

机 器 人 关节 的 期 望 速 度 。 

* /jointVelMeasure(std_msgs/Float64MultiArray) 

机 器 人 关节 的 实际 速度 。 

* /MediumSize/BodyHub/SensorRaw(bodyhub::SensorRawData) 
外 接 传感器 的 原始 数据 。 消 息 定义 ; 


uint8[] sensorReadID 
uinti6[] sensorStartAddress 
uinti6[] sensorReadLength 


ION ANDE Me ME 


int32[] sensorData 


uint8 sensorCount 


uint8 dataLength 


其 中 ，sensorReadID 为 外 接 传感器 ID 列表 ; sensorStartAddress 为 外 接 传感器 起 始 地 址 列表 ; 
sensorReadLength 为 外 接 传感器 数据 长 度 列表 ; sensorData 为 外 接 传感器 原始 数据 列表 ; sensor- 
Count 为 外 接 传感器 个 数 ，dataLength 为 数据 总 长 。 

* /startSimulation(std_msgs/Bool) i 

开始 仿真 。 

* /stopSimulation(std_msgs/Bool) 

停止 仿真 。 

* /pauseSimulation(std_msgs/Bool) 

暂停 仿真 。 

* /enableSyncMode(std_msgs/Bool) 

使 能 同步 仿真 模式 。 

* /triggerNextStep(std_msgs/Bool) 

触发 仿真 运行 一 帧 。 

* /sim/joint/command(std_msgs/Float64MultiArray) 

仿真 机 器 人 的 关节 角度 指令 。 

3) 提供 的 服务 

* /MediumSize/BodyHub/StateJump(bodyhub::SrvState) 

节点 状态 机 跳 转 。 服 务 定义 : 


uint8 masterID 


string stateReq 


inti6 stateRes 


其 中 ，masterID 为 占用 节点 的 ID;， stateReq 为 状态 跳 转 的 控制 字符 串 ，stateRes 为 操作 返回 
的 状态 数值 。 

* /MediumSize/BodyHub/GetStatus(bodyhub::SrvString) 

返回 节点 状态 机 的 状态 ， 返 回 关 节 指 令 队 列 的 长 度 。 服 务 定义 : 


string str 


string data 
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uint32 poseQueueSize 


其 中 ，str 为 请 求 字符 串 ， 可 任意 设置 ， 不 能 为 室 ;， data 为 状态 的 字符 串 ; poseQueueSize 为 
关节 指令 队列 的 长 度 。 

* /MediumSize/BodyHub/GetMasterID(bodyhub::SrvTLSstring) 

返回 控制 节点 的 耳 。 服 务 定义 : 


string str 


uint8 data 


其 中 ，str 为 请 求 的 字符 串 ， 可 任意 设置 ，data 为 返回 的 ID 值 。 
* /MediumsSize/BodyHub/GeUointAngle(bodyhub::SrvServoAlIRead) 


返回 机 器 人 关节 的 当前 角度 。 

* /MediumSize/BodyHub/DirectMethod/InstRead Val(bodyhub::SrvInstRead) 
i Dynamixel 设备 寄存 器 。 

* /MediumSize/BodyHub/DirectMethod/InstWriteVal(bodyhub::SrvInstWrite) 

写 Dynamixel 设备 寄存 器 。 

* /Mediumsize/BodyHub/DirectMethod/SyncWriteVal(bodyhub::SrvSyncWrite) 

同步 写 Dynamixel 寄存 器 。 

* /MediumSize/BodyHub/DirectMethod/SetServoTarPosition Val(bodyhub::SrvServoWrite) 
Bee ECOL BF Br EE 

* /MediumSize/BodyHub/DirectMethod/SetServoTarPosition Val All(bodyhub::SrvServoAIIWrite) 
设置 全 部 舵 机 目标 位 置 的 值 。 

* /MediumSize/BodyHub/DirectMethod/GetServoPositionValAll(bodyhub::SrvServoAllRead) 
获取 全 部 能 机 位 置 的 值 。 

* /MediumSize/BodyHub/DirectMethod/InstRead(bodyhub::SrvInstRead) 

i Dynamixel 设备 寄存 器 。 

* /MediumSize/BodyHub/DirectMethod/InstWrite(bodyhub::SrvInstWrite) 

写 Dynamixel 设备 寄存 器 。 

* /MediumSize/BodyHub/DirectMethod/SyncWrite(bodyhub::SrvSyncWrite) 

同步 写 Dynamixel 寄存 器 。 


* /MediumSize/BodyHub/DirectMethod/SetServoTarPosition(bodyhub:;SrvServoWrite) 
设置 单个 能 机 目标 位 置 的 角度 。 
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* /MediumSize/BodyHub/DirectMethod/SetServoTarPositionAll(bodyhub::SrvServoAll Write) 
设置 全 部 舵 机 目标 位 置 的 角度 。 

* /MediumSize/BodyHub/DirectMethod/GetServoPositionAll(bodyhub::SrvServoAllRead) 
获取 全 部 舵 机 位 置 的 角度 。 

* /MediumSize/BodyHub/DirectMethod/SetServoLockState(bodyhub::SrvServoWrite) 
设置 单个 能 机 的 扭矩 开关 。 

* /MediumSize/BodyHub/DirectMethod/SetServoLockStateAll(bodyhub::SrvServoAll Write) 
BEARRIK. | 

* /MediumSize/BodyHub/DirectMethod/GetServoLockStateAll(bodyhub::SrvServoAllRead) 
获取 全 部 舵 机 的 扭矩 开关 状态 。 

* /MediumSize/BodyHub/RegistSensor(bodyhub::SrvInstWrite) 

注册 外 接 传感器 。 服 务 定 义 : 


pe itemName 


|uint8 dxlID 
| float64 setData 
| 


| 
| bool complete 


其 中 ,itemName 为 注册 的 传感器 的 名 称 ;dxlID 为 要 获取 的 传感器 数据 的 寄存 器 地 址 ; setData 
为 要 获取 的 传感器 数据 的 地 址 长 度 ，complete 为 注册 成 功 与 否 的 结果 。 

* /MediumSize/BodyHub/DeleteSensor(bodyhub::SrvTLSstring) 

删除 注册 的 外 接 传感器 。 服 务 定义 : 


string str 


| nint8 data 


其 中 ，str 为 要 删除 的 传感器 名 称 ，data 为 删除 的 结果 。 
4) 其 他 参数 

* poseOffsetPath(std::string, default:"") 

机 器 人 零点 文件 路 径 。 

* poselnitPath(std::string, default:"") 

机 器 人 初始 姿势 文件 路 径 。 

* sensorNameIDPath(std::string, default:"") 

机 器 人 外 接 传感器 的 信息 文件 路 径 。 
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* simenable(bool, default:false) 

仿真 模式 的 控制 变量 。 

更 多 内 容 ， 读 者 可 以 通过 阅读 BodyHub 的 说 明文 档 来 了 解 。 
4. 释放 BodyHub 节点 


释放 节点 的 占用 和 占用 基本 一 样 ， 向 /MediumSize/BodyHub/StateJump 服务 发 送 释 放 请 求 即 
可 ， 操 作 如 下 : 


$ rosservice call /MediumSize/BodyHub/StateJump "masterID: 6 


> stateReq: 'reset'" 


或 


$ rosservice call /MediumSize/BodyHub/StateJump 6 reset 


若 返 回 以 下 内 容 ， 则 释放 成 功 ， 如 图 5.39 所 示 。 


stateRes: 21 


5.39 ”占用 状态 机 


若 返 回 其 他 内 容 ， 则 释放 失败 。 

至 此 ， 我 们 知道 了 如 何 启用 BodyHub 节点 ， 并 占用 状态 机 ， 读 者 可 能 还 没有 理解 为 什么 要 
这 样 做 ， 大 家 现在 只 需要 知道 在 其 他 节点 对 机 器 人 进行 操作 之 前 需要 先 获取 该 节点 的 控制 权 才 
能 进行 下 一 步 操 作 即 可 。 后 面 我 们 将 通过 编写 程序 及 节点 状态 机 对 仿真 模型 进行 控制 。 


5.2.3 ”关节 运动 控制 


机 器 人 的 基础 就 是 运动 ， 而 人 形 机 器 人 是 所 有 机 器 人 中 运动 控制 最 为 复杂 的 一 种 ， 控 制 机 
器 人 完成 特定 动作 之 前 ， 我 们 首先 要 学 习 一 下 如 何 控制 机 器 人 的 关节 运动 。 根 据 前 面 讲 到 的 知 
识 ， 读 者 可 以 想到 使 用 ROS 编程 ， 通 过 节点 进行 控制 ， 具 体 编程 语言 包括 C++、Python 等 。 这 
里 我 们 采用 Python 作为 编程 语言 ,通过 官方 提供 的 ActExecPackageNode 节点 ， 完 成 机 器 人 的 关 
节 运 动 控制 。 
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1. ActExecPackage 简介 

在 进行 实际 操作 之 前 ， 我 们 首先 对 ActExecPackage &13EfT— 3E IF] T ff. 

ActExcPackage 是 动作 执行 功能 包 ， 可 执行 机 器 人 自 定义 动作 ,通过 Python 实现 ， 运 行 给 定 
名 称 的 动作 包 ， 完 成 机 器 人 关节 控制 。 

它 所 包含 的 话题 ， 主 要 是 发 布 话题 : 

/MediumSize/ActPackageExec/Status 

当 状 态 发 生变 化 时 ， 发 送 新 的 状态 

它 所 包含 的 服务 有 3 个 : 

(D /MediumSize/ActPackageExec/actNameString SrvActScript.srv。 通 过 该 服务 可 使 用 对 应 机 
器 人 动作 名 称 运 行 对 应 的 机 器 人 动作 文件 。 

@ /MediumSize/ActPackageExec/StateJump 依据 节点 当前 状态 和 跳 转 命令 变换 状态 SrvState. 
srv, 跳 转 命令 : StateEnum::setStatus 设置 当前 的 节点 占用 状态 ; StateEnum::break 如 果 处 于 running 
状态 ， 正 在 运行 .py， 中 断 当前 运行 脚本 ， 并 跳 转 至 状态 pause; StateEnum::stop 如 果 处 于 pause 
状态 ， 跳 转 至 ready 状态 ; StateEnum::reset 从 节点 恢复 到 pre. ready 状态 ， 直 接 释 放 控 制 权 。 

@ /MediumSize/ActPackageExec/GetStatus SrvString.srv 获取 当前 所 处 的 运行 状态 。 

话题 与 服务 ， 如 图 5.40 所 示 。 


图 5.40 话题 与 服务 


2. ActExecPackage 话题 与 服务 
这 里 我 们 深入 了 解 一 下 ActExecPackage 的 话题 与 服务 。 
1) 发 布 的 话题 

* /MediumSize/ActPackageExec/Status(std msgs/UInt16) 
动作 执行 节点 当前 状态 的 数值 。 状态 定义 


self.StateStyle = { 


'1nit' :"20, 


'preReady' : 21, 


a ee n 


€ 
"Yeady' 1 22, 


'running' : 23, 
'pause' : 24, 
'stoping' : 25, 


‘error’ + 26, 


2) 提供 的 服务 
* /MediumSize/ActPackageExec/GetStatus(SrvString) 
返回 节点 状态 机 的 状态 ; 返回 关节 指令 队列 的 长 度 。 服 务 定义 : 


string str 
string data 


uint32 poseQueueSize | 


其 中 ，str 为 请 求 字 符 串 ， 可 任意 设置 ， 不 能 为 空 ，data 为 状态 的 字符 串 ; poseQueueSize 为 
关节 指令 队列 的 长 度 。 

* /MediumSize/ActPackageExec/StateJump(SrvState) 

节点 状态 机 跳 转 。 服 务 定义 : 


uint8 masterID 
string stateReq 


int16 stateRes 


其 中 ，masterID 为 占用 节点 的 ID; stateReq 为 状态 跳 转 的 控制 字符 串 ; stateRes 为 操作 返回 
的 状态 数值 。 

* /MediumSize/ActPackageExec/actNameString(SrvActScript) 

需要 执行 的 动作 指令 字符 。 服 务 定 义 : 


string actNameReq 


string actResultRes 


其 中 ，actNameRegq 为 指令 名 称 ; actResultRes 为 执行 结果 。 


See ui 


3. TrajectoryPlanning 类 

我 们 了 解 了 ActExecPackage 的 话题 和 服务 ， 那 么 是 如 何 通过 Python 来 进行 控制 的 呢 ? 这 里 ， 
官方 提供 了 TrajectoryPlanning 类 。 具 体 如 下 : 

TrajectoryPlan.py 文件 为 使 用 自动 贝 塞 尔 做 动作 轨迹 规划 的 代码 。 

TrajectoryPlanning 类 中 ， 若 i 为 任意 一 个 目标 值 ， 则 i—1l 为 初始 值 ， 计 1 为 下 一 个 目标 值 ， 
i—1 和 i 值 之 间 的 轨迹 为 第 i 段 轨迹 。 生 成 第 i 段 轨迹 时 ， 需 要 i-1、i、i+l 三 个 目标 值 。 

1) 接口 简介 

类 构造 函数 : 


def __init__(self, numberOfTra=22, daltaX-10.0) | 


BR GnumberOfTra: 规划 的 轨迹 组 数 〈 关 节 数 )。 
参数 @daltaX: 轨迹 插值 的 时 间 间 隔 ， 单 位 为 ms。 
设置 轨迹 插值 的 间隔 : 


def setDaltaX(self, daltaX) 


参数 @daltaX: 间隔 时 间 ， 单 位 为 ms. 
设置 目标 值 之 间 的 间隔 : 


r 
| det setInterval(self, v) | 


参数 ev. 间隔 时 间 ， 单 位 为 ms。 
开始 轨迹 生成 的 准备 ， 传 入 初始 值 和 第 一 组 目标 值 : 


def PlanningBegin(self，firstGroupValue，secondGroupValue) | 


参数 @firstGroupValue: 初始 值 。 
参数 @secondGroupValue: 第 一 组 目标 值 。 
传 入 下 一 组 目标 值 it1， 并 返回 上 两 组 目标 值 i-1 和 i 之 间 的 第 i 段 轨迹 : 


def planning(self, nextGroupValue) 


参数 @nextGroupValue; 下 一 组 目标 值 。 
返回 值 ，list 类 型 ， 上 两 组 目标 值 生成 的 轨迹 。 
生成 最 后 一 段 轨迹 (i Al i+] ZIAD): 


def planningEnd(self) | 


返回 值 : list 类 型 ， 生 成 的 最 后 一 段 轨迹 列表 。 
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2) 示例 代码 


# 创建 一 个 实例 ， 规 划 22 自 曲线， 轨迹 点 插值 间隔 为 Oms 
tpÜbject = TrajectoryPlanning(22,10.0) 


# 设置 目标 点 之 间 的 时 间 间 隔 为 1000ms 

tp0bject .setInterval (1000.0) 

# 传 入 初始 值 和 第 一 组 目标 值 
tpÜbject.planningBegin(poseList[0], poseList[1]) 


# 修改 目标 点 之 间 的 时 间 间 隔 为 1500ms 
tp0bject.setInterval (1500.0) 
# 传 入 下 一 组 目标 值 ， 并 返回 上 两 组 目标 值 之 间 的 轨迹 


trajectoryPoint = tp0bject.planning(poseList[2]) 


# 修改 目标 点 之 间 的 时 间 间 隔 为 2000ms， 若 不 调用 此 函数 ， 目 标点 的 间隔 保持 为 1500ms 
tpÜbject.setInterval(2000.0) 

# 传 入 下 一 组 目标 值 ， 并 返回 上 两 组 目标 值 之 间 的 轨迹 

trajectoryPoint = tp0bject.planning(poseList [3]) 


# 返回 最 后 一 段 轨迹 
trajectoryPoint = tpÜbject.planningEnd() 


| S 


4. ActExecPackage 运行 

对 ActExecPackage 有 一 定 认 识 之 后 ， 我 们 开始 启动 它 吧 。 

首先 ， 根 据 4.2.2 的 内 容 ， 先 启动 BodyHub 节点 ， 这 里 ， 我 们 暂时 不 需要 占用 状态 机 ， 仅 启 
动 节点 即 可 。 图 5.41 所 示 为 运行 BodyHub 节点 。 

机 器 人 进入 仿真 状态 后 ， 我 们 进入 终端 ， 输 入 如 下 命令 ， 启 动 ActExcPackageNode 6 节点， 


$ rosrun actexecpackage ActExecPackageNode.py 


图 5.42 所 示 为 运行 ActExcPackageNode 节点 。 
因为 ActExecPackage 自 带 状态 机 管理 , 所 以 我 们 使 用 上 级 节点 通过 /MediumSize/ActPackage- 
Exec/StateJump 服务 占用 启动 的 节点 。 这 里 我 们 申请 ID 为 6: 


$ rosservice call /MediumSize/ActPackageExec/StateJump "masterID: 6 


ae 
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> stateReq: 'setStatus'" 


$ rosservice call /MediumSize/ActPackageExec/StateJump 6 setStatus 


TUE VY E 


bn t " 
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图 5.41 运行 BodyHub 节点 


图 5.42 运行 ActExcPackageNode 节点 
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图 5.43 所 示 为 占用 节点 。 


543 SHWA 


与 BodyHub 同 理 ， 返 回 stateRes:22 占用 成 功 ， 否 则 占用 失败 。 

5. 机 器 人 关节 控制 实践 

占用 成 功 后 ， 上 级 节点 通过 /MediumSize/ActPackageExec/actNameString 服务 , 运行 机 器 人 自 
定义 动作 ， 这 里 ， 我 们 通过 一 个 案例 进行 讲解 。 

HG, 在 /src/actexecpackage/cong 文件 夹 下 , 新建 一 个 Python 文件 , 并 将 如 下 代码 复制 进去 ， 
命名 为 TrajectoryPlanExample.py。 


#!/usr/bin/env Python 
# -*- coding: UTF-8 -*- 
import rospy 
import time 
from bodyhub.msg import * 
from bodyhub.srv import * 
from TrajectoryPlan import * | 
NodeControlId = 6 . 
def GetBodyhubStatus(): 
rospy.wait for service('MediumSize/BodyHub/GetStatus!') 
client - rospy.ServiceProxy('MediumSize/BodyHub/GetStatus', SrvString) 
response - client('get') 


return response 


def SetBodyhubStatus(id, status): 
rospy.wait for service('MediumSize/BodyHub/StateJump') 


client = rospy.ServiceProxy('MediumSize/BodyHub/StateJump', SrvState) 
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client(id, status) 


def GetbodyhubControlld(): 
rospy.wait for service('MediumSize/BodyHub/GetMasterID') 
client - rospy.ServiceProxy('MediumSize/BodyHub/GetMasterID', SrvTLSstring) 
response - client('get') 


return response.data 


def GetJointPosition(jointIdList): 
status = GetBodyhubStatus() 
if status.data -- 'preReady': 
SetBodyhubStatus(NodeControlld, 'setStatus') 
elif status.data == 'ready' or status.data == 'running' or status.data == 
'pause': 
if GetbodyhubControllId() != NodeControlld: 
return False 
else: 
return False 
rospy.wait for service('MediumSize/BodyHub/DirectMethod/GetServoPositionAll') 
client-rospy.ServiceProxy('MediumSize/BodyHub/DirectMethod/GetServoPositionAll', 
SrvServoAllRead) 
response = client(jointIdList, 0) 
SetBodyhubStatus(NodeControlld, 'reset') 


return response.getData 


def ClearList(list): 
while len(list) > 0: 
list.pop() 


def NumberToPoint(list,daltaX): 
xt - 0.0 
for m in range(len(list)): 
for n in range(len(list[m])): 
list[m][n] = Point(xt, list [m] [n]) 
xt = xt + daltaX 
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def SendTrajectory(pub, trajectoryPoint): 
for m in range(len(trajectoryPoint[0]1)): 
jointPosition - [] 
for n in range(len(trajectoryPoint)): 


jointPosition, append(trajectoryPoint [n] [m] . y) 
pub.publish(positions-jointPosition, mainControlID-NodeControlId) 
def WaitTrajectoryExecOver(): 
status = GetBodyhubStatus() 
while status.poseQueueSize > 5: 
status = GetBodyhubStatus() 


== ! main .': 


poseList = [ 
[0,0,0,0,0,0, 0,0,0,0,0,0, 0,-75,-10, 0,75,10, 0,0, 0,0], 


[0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0, -90,75,10, 0,0, 0,0], 
[0,0,0,0,0,0, 0,0,0,0,0,0, 0,-75,-10, 90,75,10, 0,0, 0,0], 


[0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0, -90,75,10, 0,0, 0,0], 
[0,0,0,0,0,0, 0,0,0,0,0,0, 0,-75,-10, 90,75,10, 0,0, 0,0], 


[0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0, -90,75,10, 0,0, 0,0], 
[0,0,0,0,0,0, 0,0,0,0,0,0, 0,-75,-10, 0,75,10, 0,0, 0,0], 
# idList = [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22] 


# poseList[0] = list(GetJointPosition(idList)) 


jointPositionPub = rospy.Publisher('MediumSize/BodyHub/MotoPosition' , 


JointControlPoint, queue_size=500) 
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rospy.init_node('TrajectoryPlanExample', anonymous=True) 


time.sleep(0.2) 
tpÜbject = TrajectoryPlanning(22,10.0) 
while not rospy.is shutdown(): 

status = GetBodyhubStatus() 

if status.data -- 'preReady': 


SetBodyhubStatus(NodeControlld, 'setStatus') 


tpÜbject.setInterval(1000.0) 
tpÜbject.planningBegin(poseList[0], poseList[1]) 


tpÜbject.setInterval(1500.0) 


for poseIndex in range(2, len(poseList)): 


trajectoryPoint = tpÜbject.planning(poselist [poseIndex]) 


SendTrajectory(jointPositionPub, trajectoryPoint) 


WaitTrajectoryExecÜver() 
trajectoryPoint = tpÜbject.planningEnd() 
SendTrajectory(jointPositionPub, trajectoryPoint) 
WaitTrajectoryExecOver() 


SetBodyhubStatus(NodeControlld, 'reset') 


rospy.signal_shutdown('over') 


代码 保存 好 后 ， 我 们 通过 如 下 命令 ， 运 行 刚刚 设计 的 机 器 人 动作 ; 


$ rosservice call /MediumSize/ActPackageExec/actNameString "actNameReq: 'rosrun 


actexecpackage TrajectoryPlanExample.py'" 


或 


$ rosrun actexecpackage TrajectoryPlanExample.py 
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在 V-REP 中 ， 机 器 人 的 手 部 关节 开始 按照 程序 运动 起 来 ， 如 图 5.44 所 示 。 
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图 5.44 机 器 人 关节 控制 


读者 可 以 尝试 修改 poseList 中 的 数据 ， 即 修改 成 你 想 要 将 机 器 人 的 关节 转动 到 的 角度 ， 然 
后 完成 机 器 人 的 关节 运动 。 这 里 需要 注意 ， 关 节 转 动 到 的 角度 是 相对 于 机 器 人 本 体 ， 角 度 值 是 
绝对 值 ， 正 负 代 表 方向 。 

下 面 ， 请 读者 自己 尝试 一 下 吧 ， 这 里 我 们 提供 了 金鸡 独立 的 代码 ， 大 家 可 以 试 一 试 ， 看 看 
仿真 效果 如 何 。 


poseList = [ 
[0,0,0,0,0,0, 0,0,0,0,0,0, 0,-75,-10, 0,75,10, 0,0, 0,0], 
[0,0,0,0,0,0,0,0,0,0,0,0,0,-100,-30,0,100,30,0,0,0,0], 
[-8,8,0,0,0,15,-8,15,0,10,7,15,0,-100, -30,0,100,30,0,0,0,0], 
[-8,3,0,0,0,15,-8,6,-75,106,40,10,0,-10,0,0,10,0,0,0,0,0], 
[-8,3,0,0,0,15,-8,6,-75,106,40,10,0,40, -50,0,-40,50,0,0,0,0], 
[0,8,0,0,0,15,0,15,0,0,0,15,0,-10,0,0,10,0,0,0,0,0], 
[0,0,0,0,0,0,0,0,0,0,0,0,0,-100,-30,0,100,30,0,0,0,0], 
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5.2.4 ”仿真 中 的 步 态 运行 


人 形 机 器 人 的 基础 功能 就 是 行走 ， 如 何 走 得 好 是 最 困难 的 。 目 前 ， 国 内 外 的 工程 师 们 在 双 
足 人 形 机 器 人 的 步 态 算法 方面 ， 进 行 了 大 量 的 研究 。 其 主要 是 通过 测量 机 器 人 的 物理 参数 ， 如 
关节 与 关节 之 间 的 距离 、 舵 机 的 转速 等 ， 根 据 雅 可 比 矩 阵 ， 建 立 机 器 人 正 、 逆 运动 学 关系 矩阵 
求解 ， 求 解 到 每 个 能 机 的 参数 ， 再 把 参数 发 送 给 能 机 ， 完 成 机 器 人 的 运动 控制 。 除 此 之 外 ， 还 
可 以 采用 人 类 行走 的 数据 控制 机 器 人 行走 ， 实 质 是 依据 人 类 行走 的 关节 数据 来 研究 人 形 机 器 人 
的 步 态 规划 。 但 是 由 于 人 形 机 器 人 和 人 类 在 运动 学 、 动 力学 方面 存在 差异 性 ， 人 类 行走 数据 不 
能 直接 利用 在 人 形 机 器 人 上 ， 需 要 根据 动力 学 和 运动 学 的 知识 ， 通 过 不 同 的 参数 优化 方法 来 完 
成 机 器 人 的 步 态 规划 。 

我 们 在 5.2.3 节 已 经 学 习 了 如 果 对 机 器 人 的 关节 进行 控制 ， 通 过 步 态 算法 ， 即 可 实现 Roban 
机 器 人 的 双 足 步行 。 由 于 步 态 算法 涉及 的 知识 较 多 ， 这 里 我 们 不 再 展开 。 读 者 可 以 根据 本 书 第 
8 章 的 内 容 ， 进 行 深入 学 习 。 

Roban 机 器 人 的 开发 工程 师 们 已 经 设计 了 其 对 应 步 态 ， 并 存储 在 我 们 之 前 编译 的 ros 包 中 ， 
直接 调用 就 可 以 使 Roban 机 器 人 运动 起 来 。 

1. gaitcommander 简介 

gaitcommander 提供 了 一 个 控制 机 器 人 多 方向 移动 的 节点 ， 可 以 修改 步 数 和 指令 ， 完 成 机 器 
人 运动 控制 。 

它 所 包含 的 话题 ， 主 要 是 发 布 话题 : 

* /gaitCommand 

它 所 包含 的 服务 有 两 个 : 

* /gaitCommandNode/get_loggers 

* /gaitCommandNode/set_logger_level 

运行 此 节点 ， 使 用 者 即 可 通过 键盘 或 者 编程 对 机 器 人 运动 进行 控制 。 

2. 遥控 案例 

与 调用 其 他 节点 一 样 ， 我 们 首先 启动 BodyHub 节点 ， 并 且 占 用 状态 机 。 我 们 以 状态 机 ID 6 
为 例 ， 然 后 通过 以 下 指令 ， 将 状态 机 跳 转 到 walking， 如 图 5.45 所 示 。 


$ rosservice call /MediumSize/BodyHub/StateJump 6 walking 


跳 转 成 功 后 ， 我 们 发 现 机 器 人 进入 了 半 蹲 状态 。 
然后 ， 我 们 启动 gaitcommander 节点 ， 在 终端 中 输入 如 下 命令 : 


$ rosrun gait_command gait_command_node 
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图 5.46 所 示 为 启动 节点 。 


545 ”指令 控制 状态 转换 到 walking 


546 ”启动 节点 


启动 节点 后 ， 可 以 发 现 终端 变 成 了 带 输入 状态 。 这 时 我 们 就 可 以 通过 键盘 对 Roban 机 器 人 
进行 控制 ， 完 成 仿真 步 态 的 运行 ， 如 图 5.47 所 示 。 

具体 控制 指令 如 表 5.1 所 示 。 

不 过 ， 这 里 指令 和 步 数 是 固定 的 ， 读 者 如 果 需 要 自 定义 ， 可 以 修改 /gaitcommander/sre 文件 
夹 下 的 main.cpp 文件 ， 然 后 重新 编译 即 可 。 
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3. 自动 案例 
既然 我 们 可 以 通过 开启 gaitcommander 节点 ,使 用 按键 控制 机 器 人 运动 ， 那 么 , 我们 是 否 可 
以 通过 Python 脚本 ， 让 机 器 人 按照 我 们 预先 设 定 的 路 径 进行 移动 呢 ? 


5.47 ”键盘 控制 机 器 人 运动 


R51 控制 指令 


按键 指令 动作 

S S_command 原 地 踏步 
w W_command 前 进 

a A_command 右 移 

d D_command 左 移 

z Z. command 右 转 

c C command 左 转 

k 定制 移动 


q exit(0) 退出 
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根据 第 2 章 和 第 3 章 的 内 容 ， 这 当然 是 可 以 的 ， 那 么 让 我 们 来 实践 一 下 吧 。 
首先 ， 我 们 需要 知道 gaitcommander 节点 发 布 的 消息 是 什么 ， 这 里 我 们 查看 /rosV-REP/src/ 
gaitcommander/src 下 的 main.cpp 文件 : 


int stepCount=6; 


bool start=true; 


const Eigen::Vector3d S. command = Eigen: :Vector3d(0.00,0.00,0.0); 
const Eigen::Vector3d W command - Eigen::Vector3d(0.07,0.00,0.0); 
Eigen: :Vector3d(0.00,-0.04,0.0); 
Eigen: :Vector3d(0.00,0.04,0.0); 
const Eigen::Vector3d Z_command = Eigen: :Vector3d(0.00,0.00, 10.0); 


Eigen: :Vector3d(0.00,0.00,-10.0); 


1 


const Eigen::Vector3d A_command 


const Eigen::Vector3d D_command 


const Eigen: :Vector3d C. command 


然后 ,我 们 在 /rosV-REP/src/gaitcommander/scripts 下 新 建 一 个 Python 文件 ,命名 为 gait_test.py， 
代码 如 下 : 


#!/usr/bin/env Python 


import rospy 

import time 

from bodyhub.srv import SrvState 

from std_msgs.msg import Bool 

from std msgs.msg import Float64MultiArray 

GAIT RANGE - 0.05 

walkingPub = rospy.Publisher('/gaitCommand', Float64MultiArray, queue size-1) 


def walking client(walkstate): | 
rospy.wait for service("/MediumSize/BodyHub/StateJump") 
client = rospy.ServiceProxy("/MediumSize/BodyHub/StateJump", SrvState) 
client(2, walkstate) 


def slow walk(direction, stepnum): 
naw 
:param direction: "forward" or "backward" 
:param stepnum: int num 


:return: 


array - [0.0, 0.0, 0.0] 
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if direction == "forward": 
array[0] = GAIT_RANGE 

elif direction == "backward": 
array[0] = -1 * GAIT_RANGE 

elif direction == "leftward": 


array[1] = 1 * GAIT_RANGE 


elif direction == "rightward": 
array[1] = -1 * GAIT_RANGE 
else: 


rospy.logerr("error walk direction") 


for i in range(stepnum) : 


walkingPub.publish(data-array) 


def main(): 
rospy.init node("gait test",) 
time.sleep(2) 
walking client("setStatus") 
walking client("walking") 
slow walk("forward",6) 
slow walk("leftward",12) 
slow walk("backward",6) 
slow walk("rightward",12) 


if _ name | == ' » main .': 


main() 


if rospy.wait for message("/requestGaitCommand", Bool): 


对 ROS 工作 空间 编译 后 ， 我 们 进行 与 5.2.3 dE fi RAAR, Jao) BodyHub 和 


gaitCommander 节点 ， 如 图 5.48 所 示 。 


5.48 ”启动 节点 


venus@venus-virtualma... x [amy] 
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然后 ， 我 们 启动 刚刚 编写 的 Python 文件 ， 可 以 发 现 机 器 人 开始 运动 了 ， 并 且 运 动 轨迹 是 按 
照 我 们 刚刚 制定 的 方向 和 步 数 ， 如 图 5.49 所 示 。 


图 5.49 程序 运行 


$ python gait test.py 


下 面 ， 我 们 对 刚刚 的 Python 代码 进行 说 明 : 


#!/usr/bin/env Python 


import rospy 

import time 

from bodyhub.srv import SrvState 

from std_msgs.msg import Bool 

from std msgs.msg import Float64MultiArray 

GAIT RANGE - 0.05 

walkingPub = rospy.Publisher('/gaitCommand', Float64MultiArray, queue. size-1) 


首先 ， 我 们 引入 所 需要 的 包 及 ros 话题 和 服务 ， 并 声明 GAIT. RANGE 为 0.05， 对 于 这 个 数 
据 ， 读 者 可 以 党 试 修改 ， 这 对 后 面 的 机 器 人 步 态 仿 真有 一 定 的 影响 。 


def walking client(walkstate): 


rospy.wait for service("/MediumSize/BodyHub/StateJump") 
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r E T 


client = rospy.ServiceProxy("/MediumSize/BodyHub/StateJump", SrvState) 
client(2, walkstate) 


这 里 我 们 定义 了 状态 机 跳 转 函数 。 


def slow walk(direction, stepnum) : 


"nn" 
:param direction: "forward" or "backward" 
:param stepnum: int num 


:return: 


"n" 


| array = [0.0, 0.0, 0.0] 


if direction == "forward"; 
array[0] = GAIT_RANGE 

elif direction == "backward": 
array[0] = -1 * GAIT RANGE 

elif direction -- "leftward": 
array[i] = 1 * GAIT RANGE 

elif direction -- "rightward": 


array[i] = -1 * GAIT RANGE 
else: 
rospy.logerr("error walk direction") 


| for i in range(stepnum): 


if rospy.wait for message("/requestGaitCommand", Bool): 


walkingPub.publish(data-array) 


这 里 我 们 定义 了 步 态 运行 函数 ， 读 者 需要 注意 的 就 是 步 态 指令 与 参数 ， 需 要 与 我 们 之 前 
的 .cpp 文件 对 应 。 


def main(): 


rospy.init, node("gait test",) 
time.sleep(2) 

walking client("setStatus") 
walking client("walking") 
slow walk("forward",6) 

slow walk("leftward",12) 
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slow_walk("backward" ,6) 
slow walk("rightward",12) 


最 后 ， 我 们 定义 了 主 函 数 ， 这 里 主要 是 完成 状态 机 的 跳 转 ， 设 定 步 态 和 步 数 。 
至 此 ， 关 于 Roban 机 器 人 在 V-REP 的 关节 运动 与 步 态 运 行 就 介绍 完了 ， 读 者 快 动手 尝试 一 
下 吧 。 


5.3 V-REP 传感器 使 用 


机 器 人 传感器 在 机 器 人 的 控制 中 起 了 非常 重要 的 作用 。 正 因为 有 了 传感器 ， 机 器 人 才 具 备 
了 类 似 人 类 的 知觉 功能 和 反应 能 力 。V-REP 和 ROS 给 我 们 提供 了 丰富 的 传感器 模块 和 通信 方 
式 ， 这 里 我 们 以 视觉 传感器 和 接近 传感器 为 例 进行 讲解 。 


5.3.1 ”视觉 传感器 


机 器 视觉 是 机 器 人 进行 物体 操控 和 导航 的 一 个 重要 内 容 。 目 前 市 场 上 有 许多 2D/3D 的 视觉 
传感器 ， 而 且 大 多 数 传感器 在 ROS 中 都 有 驱动 程序 。Roban 机 器 人 提供 了 两 个 相机 : 一 个 是 位 
于 头 部 即 Realsense D435 RGBD 深度 摄像 头 ， 除 了 可 以 得 到 通常 的 RGB 图 像 之 外 还 可 获取 到 分 
辩 率 为 1280x720 像素 的 深度 信息 ， 最 高 可 以 提供 30 帧 的 RGB 图 像 以 及 90 帧 的 深度 图 像 ， 这 
给 机 器 人 进行 V-SLAM 导航 的 可 能 性 提供 了 基础 ， 男 一 个 是 标准 的 UVC 摄像 头 ， 可 以 提供 俯 
视 视角 的 图 像 信 息 。 这 里 ， 我 们 将 讨论 如 何 通 过 话题 订阅 视觉 ， 进 行 编程 。 

另外 ， 如 果 读 者 对 开源 计算 机 视觉 库 〈Open Source ComputerVision, OpenCV) 和 点 云 库 
(Point Cloud Library, PCL) 感 兴趣 ， 可 以 阅读 《ROS 机 器 人 高 效 编程 》 和 《精通 ROS 机 器 人 编 
程 》 里 面 有 详细 的 介绍 ， 这 里 我 们 不 再 展开 。 

1. Camera 简介 

首先 ， 我 们 启动 ROS， 打 开 V-REP， 并 运行 BodyHub 节点 ， 可 以 查看 到 话题 中 已 经 有 了 摄 
像 头 话题 ， 并 且 V-REP 中 提供 了 摄像 头 窗口 ， 可 以 直接 查看 ， 如 图 5.50 和 图 5.51 所 示 。 

除了 查看 获取 的 图 像 之 外 , 通过 订阅 话题 , 可 以 获得 图 像 的 参数 , 这 里 我 们 以 视觉 为 例 , 调 
用 D435 视觉 ， 通 过 如 下 命令 ， 在 另 一 个 窗口 中 显示 图 像 : 


js rosrun image_view image_view image:=<image topic> [image transport type] 


例如 : 


$ rosrun image_view image_view image:=/sim/camera/D435/colorImage 


通过 小 窗口 即 可 查看 机 器 人 所 见 情况 ， 如 图 5.52 和 图 5.53 所 示 。 
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现在 我 们 已 经 学 会 了 如 何 从 相机 中 获取 图 像 ， 下 面 开 始 学 习 使 用 ROS 和 OpenCV 进行 图 像 


Se wm 


处 理 。 

2. 使 用 cv. bridge 在 ROS 和 OpenCV 之 间 转 换 图 像 

下 面 , 我 们 将 学 习 如 何在 ROS 图 像 消息 (sensor_msgs/Image) # OpenCV 图 像 数据 (cv::Mat) 
之 间 进 行 图 像 转换 。 这 种 转换 主要 依赖 ROS 中 的 cv. bridge 软件 包 ， 它 是 vi- sion_opencv 软件 
包 集 的 一 部 分 。 在 cv_bridge 软件 包 中 ，CvBridge 库 用 来 执行 这 种 转换 。 我 们 可 以 在 代码 中 用 
CvBridge 库 执行 这 种 转换 。 图 5.54 显 示 了 如 何在 ROS 和 OpenCV 之 间 完 成 转换 。 


OpenCV | OpenCV cv::Mat 


CvBridge 
ROS Image Message 


5.54 cv bridge 


在 这 里 ，CvBridge 库 充 当 ROS 消息 与 OpenCV 图 像 互 相 转换 的 桥梁 ， 我 们 将 通过 下 面 的 例 
子 讲 解 如 何在 ROS 和 OpenCV 之 间 进 行 转换 。 

3. 使 用 ROS 和 OpenCV 进行 图 像 处 理 

本 节 中 ， 我 们 将 看 到 一 个 示例 ， 该 示例 使 用 cv bridge 从 相机 中 获取 图 像 并 且 使 用 OpenC- 
VAPI 来 转换 和 处 理 图 像 。 下 面 是 该 示例 的 工作 流程 : 

C1) 从 相机 节点 的 /sim/camera/D435/colorImage(sensor_msgs/Image) 话题 订阅 图 像 。 

(2) 用 CvBridge 将 ROS 图 像 转换 为 OpenCV 图 像 类 型 。 

(3) 用 OpenCV 的 API 处 理 图 像 ， 找 到 图 像 的 边缘 。 

(4) 将 边缘 检测 的 OpenCV 图 像 转换 为 ROS 图 像 消息 并 将 其 发 布 到 /edge_detectorprocessed_ 
image 话题 上 。 

下 面 一 步 一 步 地 介绍 该 示例 的 操作 步骤 。 

步骤 1: 创建 一 个 ROS 软件 包 。 

使 用 下 面 的 命令 创建 一 个 新 软件 包 : 


$ catkin_create_pkg cv_bridge_tutorial_pkg cv_bridge image_transport roscpp sensor_ 


msgs std_msgs 


该 软件 包 主 要 依赖 cv. bridge. image. transport 和 sensor_msgs. 
步骤 2: 创建 源 代码 文件 。 
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在 cv. bridge tutorial pkg/src 文件 夹 下 , 新 建 一 个 C++ 文件 , 命名 为 sample_cv_bridge_node. 
cpp， 代 码 如 下 : 


#include <ros/ros.h> 


#include <image_transport/image_transport .h> 
#include <cv_bridge/cv_bridge.h> 

#include <sensor_msgs/image_encodings .h> 
#include <opencv2/imgproc/imgproc.hpp> 
#include <opencv2/highgui/highgui .hpp> 


static const std::string OPENCV_WINDOW = "Raw Image window"; 
static const std::string OPENCV_WINDOW_1 = “Edge Detection"; 


class Edge_Detector 

{ 
ros: :NodeHandle nh_; 
image_transport::ImageTransport it_; 
image transport::Subscriber image_sub_; 


image transport::Publisher image pub. ; 


public: 
Edge. Detector() 
: it (nh) 


// Subscribe to input video feed and publish output video feed 

image sub. = it .subscribe("/sim/camera/D435/colorlImage", 1, 
&Edge Detector::imageCb, this); 

image pub. = it. .advertiss("/edge. detector/raw. image", 1)5 

cv: :namedWindow(OPENCV WINDOW); 


7CEdge. Detector() 
1 

cv: :destroyWindow(OPENCV WINDOW); 
} 


人 at 


void imageCb(const sensor msgs::ImageConstPtr£ msg) 


1 


cv bridge::CvImagePtr cv. ptr; 


namespace enc = sensor msgs::image encodings; 


try 
1 
cv_ptr = cv. bridge::toCvCopy(msg, sensor msgs::image. encodings::BGR8); 
} 
catch (cv_bridge::Exception& e) 
1 
ROS, ERROR("cv, bridge exception: %s", e.what()); 


return; 


// Draw an example circle on the video stream 


if (cv_ptr->image.rows > 400 && cv_ptr->image.cols > 600){ 


detect_edges(cv_ptr->image) ; 
image_pub_.publish(cv_ptr->tolmageMsg()) ; 


f 
} 
void detect_edges(cv::Mat img) 
$ 


cv::Mat src, src. gray; 


cv::Mat dst, detected edges; 


int edgeThresh - 1; 
int lowThreshold - 200; 
int highThreshold -300; 


int kernel, size - 5; 


img.copyTo(src); 
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cv::cvtColor( img, src gray, CV_BGR2GRAY ); 
cv::blur( src.gray, detected edges, cv::Size(5,5) ); 

cv::Canny( detected edges, detected edges, lowThreshold, highThreshold, 
yy 


dst = cv::Scalar::all(0); 
img.copyTo( dst, detected_edges) ; 
dst .copyTo (img) ; 


cv::imshow(OPENCV_WINDOW, src); 
cv::imshow(OPENCV_WINDOW_1, dst); 
cv: :waitKey (3); 


k$ 


int main(int argc, char** argv) 

t 
ros::init(argc, argv, "Edge Detector"); 
Edge Detector ic; 
ros::spin(); 


return 0; 


kernel_size 


步骤 3: 代码 说 明 。 
下 面 是 完整 的 代码 解释 : 


#include <image_transport/image_transport .h> 


此 段 代 码 用 image_transport 软件 包 发 布 和 订阅 ROS 中 的 图 像 。 


#include <cv_bridge/cv_bridge.h> 


#include <sensor_msgs/image_encodings.h> 


这 两 个 头 文件 包含 了 CvBridge 类 以 及 与 图 像 编码 相关 的 函数 。 
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#include <opencv2/imgproc/imgproc.hpp> 


#include <opencv2/highgui/highgui .hpp> | 


这 两 个 头 文件 包含 了 OpenCV 图 像 处 理 模块 和 GUI 模块 ， 分 别提 供 图 像 处 理 和 GUI 的 API。 


image_transport::ImageTransport it_; 


public: 

Edge_Detector() 
: it_(nh_) 

£ 
// Subscribe to input video feed and publish output video feed 
// 订阅 输入 视频 和 发 布 输出 视频 
image_sub_ = it_.subscribe("/sim/camera/D435/colorImage", 1, 

&Edge. Detector::imageCb, this); 


image pub. = it, .advertise("/edge detector/raw image", 1); 


我 们 来 仔细 研究 这 行 代码 image. transport::IAmageTransport it_。 这 行 代 码 创建 了 一 个 Image- 
Transport 对 象 实例 ， 用 于 发 布 和 订阅 ROS 图 像 。ImageTransport 的 API 信息 在 下 面 介 绍 。 

使 用 image_transport 发 布 和 订阅 图 像 : 

ROS 图 像 的 传输 与 ROS 中 发 布 者 和 订阅 者 非常 相似 ， 它 发 布 和 订阅 图 像 ， 并 且 带 有 相机 信 
息 。 虽 然 我 们 可 以 使 用 ros::Publisher 来 发 布 图 像 数 据 ， 但 是 使 用 图 像 传输 是 发 送 图 像 数据 更 有 
效 的 方式 。 

图 像 传 输 的 API 是 由 image transport 软件 包 提 供 的 。 使 用 这 些 API， 我 们 可 以 用 不 同 的 压 
缩 格式 来 传输 图 像 。 例如, 我 们 可 以 传输 未 压缩 图 像 ， 也 可 以 传输 JPEG/PNG 压缩 格式 图 像 ; 或 
者 在 单独 话题 中 ， 用 Theora 压缩 格式 来 传输 图 像 ， 还 可 以 通过 插件 添加 其 他 不 同 的 传输 格式 。 
默认 情况 下 ， 我 们 可 以 看 到 用 压缩 格式 和 Theora 格式 的 传输 。 


| inage_transport: :InageTransport 10; | 


在 这 行 代码 中 ， 我 们 创建 了 一 个 ImageTransport 类 的 实例 。 


image transport::Subscriber image. sub. ; | 


image_transport::Publisher image_pub_; 


在 这 两 行 代码 中 ， 用 image transport 对 象 声 明 了 订阅 者 和 发 布 者 对 象 ， 用 于 订阅 和 发 布 图 
像 数 据 。 
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image_sub_ = it_.subscribe("/sim/camera/D435/colorImage", 1, 
&Edge_Detector::imageCb, this) ; 


image_pub_ = it_.advertise("/edge_detector/raw_image", 1); 


上 面 的 代码 显示 了 如 何 实现 订阅 和 发 布 图 像 数据 。 


cv: :namedWindow(OPENCV_WINDOW) ; 
} 
~Edge_Detector() 
{ 
cv: :destroyWindow(OPENCV_WINDOW) ; 
hi 


上 面 的 代码 中 ，cv::namedWindow0 是 一 个 OpenCV 函数 ， 用 于 创建 GUI 窗口 ， 显 示 图 像 。 
该 函数 的 参数 是 GUI 的 窗口 名 。 在 析 构 函数 中 ， 根 据 窗口 名 来 销毁 GUI 窗口 。 

使 用 cv. bridge 将 OpenCV 的 图 像 转换 为 ROS 格式 的 图 像 。 

imageCb() 是 一 个 图 像 的 回调 函数 ， 它 用 CvBridge 的 API 将 ROS 图 像 消息 转换 为 OpenCV 
下 的 cv::Mat 类 型 数据 。 下 面 是 在 ROS 和 OpenCV 之 间 进 行 转换 的 代码 。 


void imageCb(const sensor_msgs::ImageConstPtr& msg) 


t 


cv_bridge::CvImagePtr cv. ptr; 


namespace enc = sensor msgs::image encodings; 


try 
1 ; 
cv_ptr = cv bridge::toCvCopy(msg, sensor_msgs: :image_encodings: :BGR8) ; 
is 
catch (cv bridge::Exception& e) 
1 
ROS ERROR("cv bridge exception: 4s", e.what()); 
return; 
} 


要 想 启 动 CvBridge, 首先 需要 创建 一 个 CvImage 的 实例 。 下面 的 代码 即 创建 了 一 个 CvImage 
的 指针 : 
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cv bridge::CvlImagePtr cv ptr; | 


CvImage 是 由 cv. bridge 提供 的 一 个 类 ， 由 OpenCV 图 像 、 编 码 方式 、ROS 消息 头等 信息 组 
成 。 利 用 CvImage， 可 以 很 方便 地 在 ROS 图 像 和 OpenCV 之 间 完 成 转换 。 


cv_ptr = cv_bridge::;toCvCopy (msg, sensor_msgs::image_encodings: :BGR8); | 


可 以 用 两 种 方式 来 处 理 ROS 图 像 消 息 : 使 用 图 像 的 副本 或 共享 图 像 数 据 。 使 用 图 像 副本 时 ， 
可 以 处 理 图 像 。 但 是 如 果 使 用 共享 图 像 的 指针 ， 则 不 能 修改 图 像 数据 。 我 们 可 以 用 toCvCopy() 
函数 创建 ROS 图 像 副 本 ,用 toCvShare() 函数 得 到 图 像 指针 。 在 这 些 函 数 中 , 我 们 需要 提 到 ROS 
消息 和 编码 类 型 。 


if (cv_ptr->image.rows > 400 && cv_ptr->image.cols > 600){ 


detect edges(cv ptr-»image); 
image. pub. .publish(cv.ptr-»toImageMsg()); 
Y 


这 段 代 码 从 CvImage 实例 中 提取 图 像 及 其 属性 ， 并 从 该 实例 中 访问 cv::Mat 对 象 ， 只 检查 
图 像 的 行 和 列 是 否 在 特定 范围 内 。 如 果 在 特定 范围 内 ， 则 调用 男 一 个 方法 detect_edges(cv::Mat). 
该 方法 处 理 图 像 并 显示 边缘 检测 的 结果 图 像 。 


image. pub. .publish(cv. ptr-»toImageMsg()); 


上 面 这 行 代码 在 转换 为 ROS 图 像 消息 后 发 布 边缘 检测 图 像 。 在 这 里 ， 我 们 用 toImageMsgO 
函数 将 CvImage 实例 转换 为 ROS 图 像 消 息 。 

图 像 边 缘 检 测 : 

将 ROS 图 像 转换 为 OpenCV 类 型 后 ， 将 调用 detect_edges(cv::Mat) 函数 进行 图 像 的 边缘 检 
测 ， 县 体 使 用 下 述 OpenCV 的 内 置 函数 : 


cv::cvtColor( img, src gray, CV_BGR2GRAY ); 


cv::blur( src. gray, detected edges, cv::Size(5,5) ); 
cv::Canny( detected edges, detected edges, lowThreshold, highThreshold, 


kernel size ); 


这 里 ， 调 用 evtColor() 函数 将 RGB 图 像 转 换 为 灰色 图 像 ， 调 用 cv::blur() 函数 对 图 像 进行 模 
糊 处 理 。 最 后 ， 用 Canny 边缘 检测 器 提取 图 像 中 的 边缘 。 

显示 原始 图 像 和 边缘 检测 图 像 ; 

我 们 用 OpenCV 的 imshow() 函数 显示 图 像 数 据 。 该 函数 包含 窗口 名 称 和 图 像 名 称 : 
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cv::imshow(OPENCV_WINDOW, src); 
cv::imshow(OPENCV_WINDOW_1, dst); 
cv: :waitKey (3); 


步骤 4: 编辑 CMakeLists.txt 文件 。 

下 面 是 CMakeLists.txt 文件 的 内 容 。 因 为 本 例 需 要 OpenCV 的 支持 , 所 以 要 包含 OpenCV 头 
文件 的 路 径 ， 并 将 源 代 码 与 OpenCV 库 路 径 连 接 起 来 : 
include_directories( 


${catkin_INCLUDE_DIRS} 
$40penCV. INCLUDE. DIRS) 


) 


add, executable(sample. cv bridge. node src/sample. cv. bridge. node.cpp) 


target link libraries(sample, cv bridge node 
$(catkin LIBRARIES) 
$40penCV LIBRARIES) 

) 


步骤 5: 编译 并 运行 示例 。 
使 用 catkin_make 编译 软件 包 后 ， 就 可 以 使 用 下 面 的 命令 来 运行 这 个 节点 。 
(1) 启动 BodyHub 节点 : 


$ roslaunch bodyhub bodyhub.launch sim:=true 


(2) 3811 cv. bridge 节点 ; 


$ rosrun cv bridge tutorial pkg sample. cv. bridge. node 


(3) 如 果 一 切 正常 ， 我 们 将 看 到 两 个 窗口 ， 如 图 $.55 所 示 ， 第 一 个 窗口 显示 原始 图 像 ， 如 图 
5.56 所 示 ， 第 二 个 窗口 显示 处 理 后 的 边缘 检测 图 像 。 

除了 以 上 案例 提供 的 2D 图 像 处 理 ， 还 可 以 使 用 D435 获取 的 3D 点 云 数 据 ， 进 行 更 多 的 应 
用 开发 ， 读 者 快 动手 试 试 吧 。 
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5.56 ”原始 图 像 和 边缘 检测 图 像 2 


5.3.2 ”接近 传感器 


除了 Roban 机 器 人 本 身 携带 的 传感器 ,我 们 在 V-REP 中 也 可 以 给 它 添加 新 的 传感器 ， 这 里 
我 们 以 接近 传感器 为 例 。 接 近 传感器 的 类 型 从 超声 波 到 红外 线 ， 从 光线 型 号 到 圆锥 型 号 ， 有 很 
多 种 ， 这 里 需要 根据 使 用 场景 进行 选择 ， 读 者 可 以 通过 阅读 官方 文档 进行 学 习 ， 本 节 仅 以 圆锥 
型 进行 展开 。 

选用 圆锥 型 的 原因 : 大 部 分 接近 传感器 是 最 好 、 最 精确 的 模型 ， 选 择 合适 的 精度 和 运行 模 
式 ， 其 可 以 有 更 大 的 计算 量 。 

具体 操作 如 下 : 

(1) 添加 Menu bar — Add — Proximity sensor — Cone type， 如 图 5.57 所 示 。 
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5.57 Cone type 


(2) 修改 传感器 的 位 置 〈《 即 坐标 ) 和 朝向 《 即 方向 )。 
修改 朝向 如 图 5.58 所 示 。 


5.58 BGH 


修改 位 置 如 图 5.59 所 示 。 
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5.59 ”修改 位 置 


(3) 修改 proximity sensor 〈 接 近 传感器 ) 的 参数 ， 如 图 5.60 所 示 。 
(4) 修改 proximity sensor (接近 传感器 ) 的 名 称 ， 如 图 5.61 所 示 。 
(5) 将 对 象 SensingNose 添加 到 Roban 下 并 进行 测试 ， 如 图 5.62 和 图 5.63 所 示 。 


5.60 ”修改 proximity sensor 的 参数 
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5.62 添加 到 Roban 下 


我 们 已 经 把 传感器 加 到 Roban 机 器 人 上 ， 通 过 Lua 程序 测试 ， 可 以 发 现 传感器 是 有 反应 的 。 


结合 4.1.4 的 内 容 ， 我 们 可 以 通过 程序 ， 将 传感器 的 反应 数值 通过 ROS. Topic 进行 传递 ， 进 而 影 
响 机 器 人 运动 控制 。 


这 里 涉及 一 定 的 Lua 编程 ， 读 者 需要 对 照 V-REP 提供 的 官方 函数 ， 对 照 练习 。 
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5.63 测试 
5.4 V-REP 使 用 实践 


根据 前 面 几 节 的 学 习 ， 我 们 已 经 掌握 了 如 何在 V-REP 中 控制 Roban 机 器 人 ， 并 完成 设 定 的 
任务 。 这 里 我 们 以 一 个 实例 进行 展示 。 我 们 以 国际 自主 智能 机 器 人 大 赛 线 上 赛 为 例 ， 对 其 中 的 
赛 项 进行 展示 。 

图 5.64 所 示 是 比赛 场地 的 立体 示意 图 。 在 真实 比赛 中 , 任务 出 现 的 顺序 以 及 在 每 个 任务 中 路 
面 和 其 他 物体 的 颜色 ， 都 可 能 和 图 中 显示 的 有 所 不 同 。 


5.604 ”比赛 场地 


2S 


5.4.1 ”过 坑 路 段 

本 节 首 先 介绍 机 器 人 的 第 一 个 任务 W“ 过 坑 ” 赛 项 。 过 坑 路 段 路 面 情况 ， 绿色 路 面 ， 路 宽 
(W) 60 cm， 总 长 60 cm。 路 中 央 有 一 个 方 坑 ， 长 x 9E (Li x Lo) 为 20cmx20 cm， 深 (H) 
15 cm， 如 图 5.65 所 示 。 要 求 : 直立 通过 有 坑 路 段 。 


图 5.65 过 坑 路 段 


首先 ， 在 执行 项 目 之 前 ， 需 要 对 任务 进行 分 析 。 
COD 机 器 人 从 出 发 点 ， 到 达 过 坑 路 段 。 
(2) 机 器 人 穿越 过 坑 路 段 。 


(3) 机 器 人 恢复 主 路 线 。 
这 里 我 们 首先 启动 仿真 环境 ， 并 打开 由 官方 提供 的 仿真 环境 ， 如 图 5.66 所 示 。 


图 5.66 仿真 环境 


为 了 完成 比赛 ， 这 里 我 们 建议 读者 以 脚本 的 形式 启动 BodyHub 等 节点 ， 并 调用 服务 完成 赛 
项 。 本 节 因 为 是 例 程 ， 所 以 仍 采用 前 面 几 节 介 绍 的 分 多 个 终端 进行 操作 ， 如 图 5.67 所 示 。 
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5.67 多 终端 运行 


首先 , 机 器 人 直行 前 进 , 然后 要 判断 机 器 人 是 否 到 达 过 坑 路 段 。 这 里 , 可 以 根据 距离 和 机 器 
人 步 态 预 估 到 达 所 需 的 步 数 , 也 可 以 通过 视觉 识别 过 坑 路 段 的 路 面 颜色 , 即 识别 绿色 路 面 。 本 节 
以 视觉 传感器 识别 视野 内 绿色 占 比 的 多 少 来 判读 是 否 到 达 了 绿色 过 坑 路面 。 读 者 可 以 根据 UVC 
摄像 头 图 像 ， 利 用 C++ 或 者 Python 编程 ， 计 算出 视野 内 的 绿色 占 比 ， 当 绿色 占 比 达到 阔 值 时 ， 
机 器 人 到 达 过 坑 路 段 ， 准 备 过 坑 。 

图 5.68 所 示 为 机 器 人 出 发 : 图 5.69 所 示 为 机 器 人 识别 到 过 坑 路 段 。 


5.68 机 器 人 出 发 
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图 5.69 机 器 人 识别 到 过 坑 路 段 


到 达 过 坑 路 段 后 ， 机 器 人 左 移 ， 准备 过 坑 ， 如 图 5.70 所 示 。 这 里 读者 可 以 通过 对 UVC 摄像 
头 图 像 进行 边缘 化 处 理 ， 识 别 到 机 器 人 方向 ， 左 侧 路 径 ， 并 将 机 器 人 移动 到 路 径 中 央 。 


图 5.70 机 器 人 向 左 侧 移动 


到 达 后 ， 机 器 人 前 进 ， 通 过 过 坑 路 段 ， 如 图 5.71 所 示 。 然 后 机 器 人 检测 到 达 区 域 边缘 ， 如 
图 5.72 所 示 。 识 别 到 离开 此 路 段 后 ， 机 器 人 回 到 标准 道路 中 央 ， 如 图 5.73 所 示 。 这 部 分 可 以 使 
用 我 们 前 面 的 代码 ， 仅 对 相关 参数 进行 调整 。 
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图 5.72 机 器 人 检测 到 达 区 域 边缘 
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图 5.73 机 器 人 恢复 到 标准 路 径 中 央 


a ee 


至 此 ， 机 器 人 就 完成 了 第 一 个 赛 项 ， 是 不 是 很 简单 呢 ? 读者 快 自 己 尝试 一 下 吧 ! 
5.4.2 EKER 


完成 “过 坑 ” 赛 项 后 ， 机 器 人 进入 雷 区 路 段 。 路 面 情况 : 路 面 上 随机 放 有 7 个 黑色 圆柱 ， 代 
表 地 雷 ; 地 雷 两 两 中 心间 距 (W) 大 于 或 等 于 30 em。 地 雷 直径 (CD) 为 2cm、 高 度 CHO 为 
5 cm， 如 图 5$.74 所 示 。 


574 雷 区 路 段 


要 求 : 

(CD 直立 行走 通过 ， 不 触 磁 地 雷 ， 得 20 分 。 

(2) 直立 行走 通过 ， 触 碰 地 雷 1 次 ， 得 10 分 。 

(3) 以 其 他 形式 通过 ， 得 0 分 。 

首先 ， 在 执行 项 目 之 前 ， 需 要 对 任务 进行 分 析 : 启动 ROS 节点 ， 进 入 仿真 状态 ; 选 定 机 器 
人 视角 ， 即 选用 UVC 摄像 头 ， 获 取 ROS 图 像 信息 ， 并 使 用 OpenCV 进行 图 像 处 理 ， 识 别 黑色 
地 雷 位 置 ， 设 定 黑 色 地 雷 导致 的 运动 禁区 ， 设 定 机 器 人 运动 参数 ， 根 据 识 别 到 的 地 雷 位 置信 息 
和 机 器 人 运动 信息 ， 设 计 机 器 人 运动 ， 完 成 任务 。 

完成 以 上 任务 ， 主 要 注意 的 就 是 黑色 地 雷 的 识别 ， 以 及 运动 中 的 步 态 控制 。 

示例 代码 及 解析 如 下 : 


#passMinefield_node 


#!/usr/bin/env Python 
# -*- coding: UTF-8 -*- 


# 首先 导入 程序 所 需要 的 库 和 ROS 相 关 的 消息 

# 主要 导入 了 numpy 模 块 和 cv2 模 块 。numpy 模 块 用 于 对 图 像 数 组 进行 操作 ; cv2 模 块 是 0penCV 的 Python 
# 包装 器 ， 用 来 访问 0penCV 的 Python 应 用 程序 接口 (API) 

import sys 


import time 


import sys, tty, termios 


import rospy 
import rospkg 


import cv2 
import numpy as np 
from cv_bridge import * 


from sensor_msgs.msg import * 


# 调用 官方 提供 的 Roban 功 能 包 
sys.path.append(rosPkg.RosPack() .get_path('leju_lib_pkg')) 


import motion.motionControl as mCtrl 


sys.path.append(rospkg.RosPack().get path('publiclib pkg')) 
import vision.imageProcessing as imgPrcs 


import algorithm.pidAlgorithm as pidAlg 


# 设置 占用 状态 机 节点 
nodeControlId = 2 


# 设置 步 长 
setpLength = [0.1, 0.06, 10.0] # x, y ,theta 
errorThreshold = [4.0, 2.0, 2.0] 


# 设置 PID 参 数 
xPid = pidAlg.PositionPID(p=0.0014) 
yPid = pidAlg.PositionPID(p-0.00115) 


# WU EHSVIA (REM BAR oe jk Ke) 
lowerBlack = np.array([0, 0, 0]) 
upperBlack = np.array([2, 2, 10]) 
lowerBlue = np.array([110, 192, 192]) 
upperBlue = np.array([130, 255, 255]) 


mine 


wall 


imgPrcs.ColorÜbject(lowerBlack, upperBlack) 
imgPrcs.ColorÜbject(lowerBlue, upperBlue) 
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# 将 ROS 图 像 消息 转换 为 OpenCV 的 图 像 ， 使 用 bridge.imgmsg_to_cv2(image_message, desired_ 
# encoding=:bgr81)， 具 体 见 后 续 代码 

bridge0bj = CvBridge() 

originImage = np.zeros((640,480,3), np.uint8) 

fpsTime = 0 

roiW, roiH = 240, 200 


# 赋值 ROSTopic 话 题 
imageTopic = '/sim/camera/UVC/colorImage' 


# 定义 ROS 图 像 消 息 处 理 函 数 
def imageCallback(msg) : 
global originImage, fpsTime 
try: 
originImage = bridgeObj.imgmsg_to_cv2(msg, 'bgr8') 
w, h = originImage.shape[i], originImage.shape[0] 
originImage = originlmage[h-roiH:h-40, w/2-roiW/2:w/2*roiW/2] 
except CvBridgeError as err: 


rospy.logerr(err) 


if False: 
tO = time.time() 
imgPrcs.putVisualization(originImage, mine.detection(originImage) ) 
imgPrcs.putVisualization(originImage, wall.detection(originImage)) 
ti = time.time() 


fps = 1.0/(time.time() - fpsTime) 

fpsTime = time.time() i 
imgPrcs.putTextInfo(originImage,fps,(t1-t0)*1000) 
cv2.imshow("Image window", originImage) 


cv2.waitKey(1) 


# 获取 地 雷 的 x 坐 标 ， 并 处 理 获 得 禁区 数据 
def GoToX0fMine(targetx) : 
global errorThreshold 


while not rospy.is_shutdown(): 
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result = mine.detection(originImage) 

if result['find'] == True: 
xError - targetx - result['Cy'] 
if abs(xError) < errorThreshold[0]: 

break 

xLength - xPid.run(xError) 
mCtrl.WalkTheDistance(xLength, 0, 0) 
mCtrl.WaitForWalkingDone() 


# 获取 地 雷 的 Y 坐 标 ， 并 处 理 获 得 禁区 数据 
def GoToYOfMine(targety): 
global errorThreshold 
while not rospy.is_shutdown(): 
result = mine.detection(originImage) 
if result['find'] == True: 
w, h = originImage.shape[1], originImage.shape [0] 
if result['Cx'] > w/2: 
yError 


else: 


(w-targety) - result['Cx'] 


yError = targety - result['Cx'] 
if abs(yError) < errorThreshold[0] : 
break 
yLength = yPid.run(yError) 
mCtrl.WalkTheDistance(0, yLength, 0) 
mCtrl.WaitForWalkingDone() 


# OUR E BRK 
def passMinefield(): 
while not rospy.is shutdown(): 
result = wall.detection(originImage) 
if result['find'] -- True: 
break 
result - mine.detection(originImage) 
if result['find'] -- True: 
GoToX0f£Mine (115) 
GoToYOfMine (8) 
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if 


# 定义 程序 终止 函数 
def rosShutdownHook(): 


# 定义 主 函 数 


for i in range(0, 3): 
mCtrl.SendGaitCommand(0.04, 0.0, 0.0) 
mCtrl.WaitForWalkingDone() 


mCtrl.ResetBodyhub() 


rospy.signal, shutdown('node. close') 


..ame.. == ' gmain..': 

rospy.init node('passMinefield node', anonymous-True) 
time.sleep(0.2) 

rospy . on_shutdown (rosShutdownHook) 
rospy.Subscriber(imageTopic, Image, imageCallback) 


rospy.loginfo('node runing...') 


if mCtrl.SetBodyhubTo walking(nodeControlld) == False: 
rospy.logerr('bodyhub to wlaking fail!') 
rospy.signal. shutdown('error') 
exit(1) 

time.sleep(1) 


passMinefield() 


mCtrl.ResetBodyhub() 
rospy.signal. shutdown('exit') 


下 面 ， 我 们 进行 实践 操作 ， 首 先 启动 仿真 环境 ， 并 打开 由 官方 提供 的 仿真 环境 ， 如 图 5.75 


所 示 。 


然后 ， 在 该 终端 执行 以 下 命令 ， 以 仿真 模式 启动 节点 : 


8 roslaunch bodyhub bodyhub.launch sim:=true | 


启动 Bodyhub 节点 后 ， 运 行 我 们 刚刚 编写 的 案例 程序 ; 


js Python passMinefield_node.py | 


Se ae Me 1m 


机 器 人 开始 运行 ， 并 识别 黑色 地 雷 及 躲避 ， 如 图 5.76 和 图 5.77 所 示 。 
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图 5.77 通过 雷 区 路 段 
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5.4.3 ” 踢 球 进 洞 路 段 


完成 “ 雷 区 ” 赛 项 后 ， 机 器 人 进入 踢 球 进 洞 路 段 ， 路 面 情况 : 路 面 上 放 有 一 枚 高 尔 夫 球 。 球 
洞 直径 (D) 为 10 em， 洞口 边沿 画 有 1 cm 宽 标 识 线 ， 球 洞 与 球 距 离 〈 五 ) 小 于 或 等 于 50 cm, 
如 图 5.78 所 示 。 


5.78 ” 踢 球 进 洞 路 段 


要 求 : 

(1) 可 以 任何 形式 通过 路 段 ， 但 直立 用 脚 足球 进 洞 〈 可 多 次 尝试 )， 得 20 分 。 

(2) 通过 ， 但 未 直立 用 脚 踢 球 进 洞 ， 得 0 分 。 

首先 ， 在 执行 项 目 之 前 ， 需 要 对 任务 进行 分 析 : 启动 ROS 节点 ， 进 入 仿真 状态 ， 选 定 机 器 
人 视角 ， 即 选用 UVC 摄像 头 ， 获 取 ROS 图 像 信息 ， 并 使 用 OpenCV 进行 图 像 处 理 ， 识 别 小 球 
位 置 ， 根据 识别 信息 控制 机 器 人 运动 ， 并 完成 跑 球 动作 。 

完成 以 上 任务 ， 主 要 是 图 像 处 理 及 运动 控制 ， 这 里 我 们 使 用 Python 程序 进行 实现 ， 示 例 代 
码 及 解析 如 下 : 
#kickBall_node.py 


#!/usr/bin/env Python 
# -*- coding: UTF-8 -*- 


# 首先 导入 程序 所 需要 的 库 和 R0S 相 关 的 消息 

# 主要 导入 了 numpy 模 块 和 cv2 模 块 。numpy 模 块 用 于 对 图 像 数组 进行 操作 ; cv2 模 块 是 DpenCV 的 Python 
# 包装 器 ， 用 来 访问 OpenCV 的 Python 应 用 程序 接口 CAPIO 

import sys 

import time 


import sys, tty, termios 


$8538  V-REP 使 用 概述 I> 193 


import rospy 


import rospkg 


import cv2 
import numpy as np 
from cv_bridge import * 


from sensor_msgs.msg import * 


sys.path. append (rospkg.RosPack() .get_path('leju_lib_pkg')) 


import motion.motionControl as mCtrl 


sys.path.append(rospkg.RosPack().get path('publiclib pkg')) 
import vision.imageProcessing as imgPrcs 


import algorithm.pidAlg as pidAlg 


t 设 定 仿真 模式 
SIMULATION = True 
nodeControlld = 2 


# 根据 模式 选择 PID 控 制 参数 

if SIMULATION: 
setpLength = [0.12, 0.08, 10.0] # x, y ,theta 
errorThreshold = [10.0, 10.0, 10.0] 


xPid = pidAlg.PositionPID(p-0.0016) 

yPid = pidAlg.PositionPID(p=0.001) 

aPid = pidAlg.PositionPID(p=0. 18) 
else: 


setpLength = [0.06, 0.03, 10.0] # x, y ,theta 
errorThreshold = [20.0, 20.0, 20.0] 


xPid = pidAlg.PositionPID(p-0.001) 
yPid = pidAlg.PositionPID(p-0.0003) 
aPid = pidAlg.PositionPID(p=0.09) 
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# it <HSViA {i 

lowerOrange = np.array([15, 100, 100]) 
upperOrange = np.array([25, 255, 255]) 
lowerCyan - np.array([80, 100, 100]) 
upperCyan - np.array([95, 255, 255]) 
lowerRed = np.array([0, 224, 961) 
upperRed - np.array([16, 255, 240]) 


# 根据 模式 传递 目标 物 信 息 ， 包 括 小 球 和 球 洞 
if SIMULATION: 


ball = imgPrcs.ColorÜbject(lowerÜrange, upperOrange) 
else: 
ball = imgPrcs.ColorÜbject(lowerRed, upperRed) 


hole = imgPrcs.ColorÜbject(lowerCyan, upperCyan) 


# 将 ROS 图 像 消 息 转换 为 0penCV 的 图 像 ， 使 用 bridge.imgmsg_to_cv2(image_message，desired_ 
# encoding='bgr8')， 上 具体 见 后 续 代码 

bridge0bj = CvBridge() 

originImage = np.zeros((640,480,3), ，np.uint8) 

fpsTime = 0 


# 根据 模式 ， 选 用 对 应 的 ROSTopic 
if SIMULATION: 


imageTopic = '/sim/camera/UVC/colorImage' 


else: 


imageTopic 


'/chin_camera/image' 


# 定义 ROS 图 像 消息 处 理 函 数 
def imageCallback(msg) : 
global originImage, fpsTime 
try: 
originImage = bridgeObj.imgmsg_to_cv2(msg, 'bgr8') 


except CvBridgeError as err: 
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rospy.logerr(err) 


if False: 
tO = time.time() 
imgPrcs.putVisualization(originImage, ball.detection(originImage) ) 


ti = time.time() 


fps = 1.0/(time.time() - fpsTime) 

fpsTime = time.time() 
imgPrcs.putTextInfo(originImage,fps,(ti-t0)*1000) 
cv2.imshow("Image window", originImage) 


cv2.waitKey(1) 


# 定义 机 器 人 运动 函数 ， 根 据 目标 物 位 置信 息 进行 控制 
def GoToBall(targetx,targety,targeta) : 
global errorThreshold 
while not rospy.is_shutdown(): 
result = ball.detection(originImage) 
if result['find'] != False: 


xError = targetx - result['Cy'] 
yError 
aError = targeta - result['Cx'] 
if (abs(xError) « errorThreshold[0]) and (abs(yError) « errorThreshold[1]) 
and (abs(aError) « errorThreshold[2]): 

break 
xLength = xPid.run(xError) 
yLength - yPid.run(yError) 
aLength = aPid.run(aError) 
mCtrl.WalkTheDistance(xLength, yLength, aLength) 
mCtrl.WaitForWalkingDone() 


else: 


targety - result['Cx'] 


rospy.logwarn('no ball found!') 
time.sleep(0.5) 


# 定义 模拟 准备 踢 球 函数 
def SimPrepareKickBall(targetx,targety,targeta) : 
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global errorThreshold 
while not rospy.is_shutdown(): 
resulti - ball.detection(originImage) 
result2 = hole.detection(originImage) 
if (resulti['find'] != False) and (result2['find'] != False): 
xError = targetx - resulti['Cy'] 
yError = targety - resulti['Cx'] 
aError = targeta - result2['Cx'] 
if (abs(xError) < errorThreshold[0]) and (abs(yError) < errorThreshold[i]) 
and (abs(aError) < errorThreshold[2]): 
break 
xLength = xPid.run(xError) 
yLength = yPid.run(yError) 
aLength = aPid.run(aError) 
mCtrl.WalkTheDistance(xLength, yLength, aLength) 
mCtrl.WaitForWalkingDone() 


i] 


# 定义 模拟 跑 球 函数 

def SimkickBall(): 
global setpLength 
GoToBal1(330.0,320.0,320.0) 
SimPrepareKickBall(360.0,280.0,285.0) 
mCtrl.SendGaitCommand(setpLength[0], 0.0, 0.0) 
mCtrl.WaitForWalkingDone() 


# 定义 任务 完成 后 ， 关 闭 节 点 函数 
def rosShutdownHook() : 
mCtrl.ResetBodyhub() 


rospy.signal, shutdown('node. close') 


if |, name. == ' main  ': 
# 启动 节点 并 获取 ROS 图 像 信息 


rospy.init_node('kickBall_node', anonymous=True) 


time.sleep(0.2) 
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rospy.on_shutdown(rosShutdownHook) 
rospy.Subscriber(imageTopic, Image, imageCallback) 


rospy.loginfo('SIMULATION: 4s',SIMULATION) 
rospy.loginfo('node runing...') 


# 判断 BodyHub 节 点 是 否 启 动 


if mCtrl.SetBodyhubTo walking(nodeControlld) == False: 


rospy.logerr('bodyhub to wlaking fail!') 
rospy.signal shutdown('error') 
exit(1) 
time.sleep(2) 
while not rospy.is shutdown(): 
if SIMULATION: 
SimkickBall() 
mCtrl.ResetBodyhub() 
rospy.signal_shutdown('exit') 
else: 
GoToBall (240.0, 260.0,260.0) 
mCtrl.SendGaitCommand(0.06, 0.0, 0.0) 
mCtrl.WaitForWalkingDone() 
mCtrl.SendGaitCommand(0.12, 0.0, 0.0) 
mCtrl.WaitForWalkingDone() 


下 面 ， 进 行 实践 操作 ， 首 先 启动 仿真 环境 ， 并 打开 由 官方 提供 的 仿真 环境 ， 如 图 5.79 所 示 。 


然后 ， 在 该 终端 执行 以 下 命令 ， 以 仿真 模式 启动 节点 : 


js roslaunch bodyhub bodyhub.launch sim:-true 


局 动 BodyHub 节点 后 ， 运 行 我 们 刚刚 编写 的 案例 程序 : 


python kickBall_node.py 


机 器 人 开始 运行 ， 并 寻找 目标 小 球 及 踢 球 进 洞 ， 如 图 5.80 和 图 5.81 所 示 。 
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图 5.81 跑 球 进 洞 


Roban 机 器 人 运动 控制 基础 


CHAPTER 6 


Roban 机 器 人 由 头 、 躯 干 、 臂 、 手 、 腿 、 足 等 部 件 组 成 ， 连 接 两 个 部 件 的 是 关节 。 大 多 数 相 
连 部 件 之 间 可 以 在 两 个 甚至 三 个 方向 上 做 相对 运动 ， 每 个 方向 上 的 运动 都 是 通过 电机 驱动 机 械 
结构 完成 的 。 本 章 首先 介绍 Roban 的 关节 结构 ， 然 后 介绍 控制 关节 及 运动 的 编程 方法 。 


6.1 关节 


Roban 机 器 人 使 用 旋转 集合 横 滚 Croll) WAN pitch 和 偏转 (yaw) 表示 运动 姿态 ， 分 别 
对 应 绕 z. y 和 z 轴 方 向 上 的 旋转 。 每 个 关节 由 关节 ID 代表 其 所 处 的 位 置 ， 如 图 6.1 所 示 。 在 描 
述 关节 的 运动 范围 时 ， 沿 旋转 轴 顺 时 针 方向 转动 为 负 ， 逆 时 针 转 动 为 正 ， 单 位 为 度 或 弧度 。 


图 6.1 Roban 机 器 人 关节 
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6.1.1 AK 


头 部 关节 包括 做 低头 、 仰 头 动 作 的 22 号 关节 以 及 做 转 头 动作 的 21 号 关节 。 其 中 , 低头 ( 沿 
y 轴 道 时 针 方 向 ) 的 最 大 幅度 为 35°; 仰 头 的 最 大 幅度 是 —24° 。 

头 部 左 转 〈 沿 z 轴 逆 时 针 方向 ) 的 最 大 幅度 是 90"， 右 转 的 最 大 幅度 是 90°* 。 以 弧度 表示 头 
部 运动 范围 时 ，22 号 关节 的 范围 是 [一 0.418, 0.6108], 21 号 关节 的 范围 是 [—1.57, 1.57]. HFK 
部 两 个 关节 相互 耦合 的 影响 ， 头 部 在 同时 做 左右 转动 和 低头 动作 时 ， 动 作 范围 会 有 所 变化 。 头 
部 关节 运动 情况 如 图 6.2 所 示 。 


62 KRR HRA 1 


头 部 左右 运动 的 关节 限 位 如 图 6.3 所 示 。 


63 头 部 关节 限 位 2 
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612 ”手臂 关节 


臂 部 肢体 通过 肩 部 与 躯干 相连 ， 包 括 肩 关 节 、 肘 关节 和 手 ， 臂 部 所 有 关节 都 是 左右 对 称 的 ， 
做 绕 ac 轴 旋 转 的 相同 关节 动作 时 ， 左 右 两 侧 旋 转角 度 互 反 。 臂 部 关节 运动 范围 如 图 6.4 所 示 。 

(1) ID8 舵 机 肩 关 节 ， 执 行 左右 臂 侧 上 举动 作 〈 绕 Hh), 动作 幅度 范围 左 臂 为 [0"，128?]， 
AMA [-128°, 0°], MAEVE A [0, 2.233] 和 [一 2.233, 0]. 


64 NX 


(2) ID7 肩 关 节 , 执行 左右 臂 经 体 前 前 摆 或 后 摆动 作 〈 绕 y HD, 动作 幅度 范围 左 辟 为 [180。， 
—90*], AA [90° —90°, 180°] ， 弧 度 范围 为 [3.14, —1.57] 和 [1.57, —3.14]. 

(3) ElbowRoll, 肘 关 节 ， 执 行 左 右 臂 弯 肘 动作 〈 绕 zx 轴 )， 包 括 LEIbow Roll, REI bow Roll , 
动作 幅度 范围 左 肘 为 [一 98°, 5°], AINA [5°, —98°] 弧度 范围 为 [一 1.71, 0.087] 和 [—0.087, 1.71], 
参见 图 6.5。 


6.5 肘 关 节 限 位 


ao BAM ABR 


6.1.3 RD 


LUBE RENEE Sok, PAF PSEA. BK ese E on Ed6.69T71s 
(1) ID1 髋 关节 ,执行 左 腿 外 转 或 内 转动 作 ( 绕 z 轴 旋 转 ) 运动 幅度 范围 为 [一 90°, 90°], 3f 
度 范围 为 [一 1.57, 1.57]， 如 图 6.6 所 示 。 


6.6 TX SAI 


(2) ID2 BOE, PUTAA UA ME CES a 轴 旋 转 ) 运动 幅度 范围 为 60°, 一 28°]， 弧 度 范围 
为 [一 1.04, 0.49]， 如 图 6.7 所 示 。 
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6.1.4 BRED D 


HB EB SELL SS ADS SAE, GLISESOR T. SCARS, MERAK TME A 
对 称 的 。 
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CD ID3 HES, 执行 腿 部 前 后 摆动 作 , 用 于 将 整 条 腿 进行 前 后 的 摆动 , 运动 幅度 范围 为 [72。， 
一 80°]， 弧 度 范围 为 [1.256, 一 1.396]， 如 图 6.8 所 示 。 

(2) IDA 膝 关 节 ， 执 行 腿 部 前 、 后 摆动 作 ， 用 于 将 整 条 小 腿 进行 前 、 后 摆 ， 和 运动 幅度 范围 为 
[6°, 一 100°?]， 弧 度 范围 为 [0.785, 一 1.745]， 如 图 6.9 所 示 。 


68 MATRES 


69 NEXT 


(3) IDS RXT, dU UE ESE, AP RATE, 运动 幅度 范围 为 [45?, 一 63?]， 
弧度 范围 为 [0.785, 一 1.099]， 如 图 6.10 所 示 。 

(4) ID6 躁 关 节 ， 执 行 脚 部 左右 摆动 作 ， 用 于 将 脚 部 左右 摆 , 运动 幅度 范围 为 [30°, 一 30°]， 
弧度 范围 为 [0.5235, 一 0.5235]， 如 图 6.11 所 示 。 
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图 6.10 BRATZ 1 图 6.11 EXTRA 2 
6.1.5 ”伺服 电机 


Roban 为 了 更 好 地 适应 不 同 关节 的 需求 ， 采 用 了 3 种 不 同类 型 的 直流 伺服 电机 ， 如 表 61 
所 示 。 


表 6.1 多 种 直流 伺服 电机 


xw. FE 
DNE. 


说 明 : 

(1) 为 了 增加 扭力 ， 每 种 伺服 电机 上 都 加 有 减速 箱 ， 通 过 与 伺服 电机 连接 的 微型 齿轮 降低 
转速 ， 对 于 不 同类 型 的 伺服 电机 ， 其 减速 比 各 不 相同 。 

(2) 转 矩 ， 简 单 地 说 ， 就 是 指 转动 的 力量 的 大 小 。 转 搜 是 一 种 力矩 ， 在 物理 学 中 ,力矩 的 定 
Mi: JD8—27) x JJ. 

这 里 的 力 臂 可 以 被 看 作 伺服 电机 所 带动 的 物体 的 转动 半径 。 转 矩 的 国际 单位 是 N «m. 

堵 转 转 矩 和 标 称 转 矩 反映 了 伺服 电机 在 启动 和 正常 工作 状态 下 驱动 力 的 大 小 。 堵 转 转 矩 是 
指 当 电 机 转速 为 零 〈 堵 转 ) 时 的 转 矩 ， 如 膝 关 节 电 机 在 启动 或 维持 半 蹲 状态 时 都 处 于 堵 转 状态 。 
额定 转 和 矩 是 电机 可 以 长 期 稳定 运行 的 转 矩 。 

头 部 关节 和 手 部 关 他 需要 带动 的 肢体 较 轻 ， 采 用 转 矩 最 小 的 电机 。 

臂 关 节 和 跨 步 偏 转 关节 使 用 转 矩 中 等 的 伺服 电机 。 

腿 部 需要 支撑 Roban 全 身 的 重量 ， 腿 关节 使 用 转 矩 最 大 的 电机 ， 同 时 自身 的 减速 比 也 会 做 
出 一 定 的 调整 ， 使 关节 的 输出 扭矩 可 以 满足 机 器 人 的 使 用 要 求 。 
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(3) 位 置 检测 。 所 有 的 关节 都 是 伺服 控制 的 机 构 。 也 就 是 说 ， 传 输 给 电机 的 力 或 力矩 指令 
都 是 根据 检测 到 的 关节 位 置 与 期 望 位 置 之 间 的 差 值 而 给 定 的 。 这 就 要 求 每 个 关节 都 要 有 一 定 的 
位 置 检测 装置 。Roban 的 位 置 检测 都 是 采用 电位 器 实现 的 ， 电位 器 和 减速 箱 的 输出 轴 固 连 , 根据 
输出 轴 的 位 置 检测 可 以 计算 得 到 真实 的 关节 转角 ， 而 控制 系统 根据 实际 关节 转角 和 给 定 关节 转 
角 的 差 值 ， 然 后 通过 PID 控制 算法 计算 电压 输出 的 信号 。 

(4) 电流 控制 。 机 器 人 腿 部 关节 的 电流 较 大 ， 因 此 腿 部 电机 控制 板 上 都 有 电流 传感器 。 为 了 
保护 电机 、 电 路 板 和 关节 的 机 械 部 分 ， 每 个 关节 都 有 电流 限制 。 如 果 电 流 达 到 限制 并 持续 ， 会 
触发 保护 机 制 ， 直 接 切断 当前 舵 机 的 控制 输出 ， 从 而 达到 保护 机 器 人 本 体 的 目的 。 


6.2 ”完整 动作 执行 
为 了 更 加 方便 地 执行 预先 定义 好 的 动作 ，Roban 机 器 人 提供 了 ActExecPackage 包 来 对 机 器 


人 动作 包 进 行 运动 控制 。 下 面 将 详细 说 明 这 个 包 的 用 法 。 首 先 介绍 机 器 人 动作 包 执 行 的 状态 机 ， 
状态 转换 如 图 6.12 所 示 。 


图 6.12 ActExecPackage 状态 转换 图 


这 个 状态 转换 结构 是 为 了 确保 当前 阶段 只 能 有 一 个 节点 对 这 个 动作 执行 节点 进行 占用 。 下 
面 简介 示例 动作 文件 的 运行 过 程 。 
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这 个 节点 对 应 的 ROS 接口 说 明 如 下 。 
提供 的 话题 ( 见 表 6.2). 


R62 话题 


‘init’: 20 


'preReady': 21 
/MediumSize/ActPackageExec/Stat em 
ediumolz ctFackagelxec/»tatus ES TORT: j 
à 动作 执行 节点 当前 状态 的 数值 。 running: 23 
(std msgs/UInt16) 
‘pause’: 24 
‘stoping’: 25 


'error: 26 


ERRE MLA 6.3). 


R63 服务 

名 称 说 明 
/MediumSize/ActPackageExec/GetStatus 返回 节点 状态 机 的 状态 ; 
(SrvString) 返回 关节 指令 队列 的 长 度 
/MediumSize/ActPackageExec/StateJump 节点 状态 机 跳 转 
(SrvState) 
Se ene 需要 执行 的 动作 指令 字符 
(SrvActScript) 


在 终端 输入 以 下 命令 ， 启 动 节点 : 


rosrun actexecpackage ActExecPackageNode.py 


使 用 上 级 节点 通过 /MediumSize/ActPackageExec/StateJump 服务 占用 启动 的 节点 ; 


rosservice call /MediumSize/ActPackageExec/StateJump "masterID: 1 
stateReq: 'setStatus'" 


返回 stateRes: 22 占用 成 功 ， 否 则 占用 失败 。 
上 级 节点 通过 /MediumSize/ActPackageExec/actNameString 服务 ， 运 行 机 器 人 自 定义 动作 : 


rosservice call /MediumSize/ActPackageExec/actNameString "actNameReq: 'rosrun 


actexecpackage TrajectoryPlanExample.py'" 


动作 执行 完成 ， 使 用 以 下 命令 释放 动作 执行 节点 : 
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rosservice call /MediumSize/ActPackageExec/StateJump "masterID: 1 


stateReq: 'reset'" 


以 上 对 这 个 节点 的 操作 是 通过 命令 行 所 实现 的 ， 这 些 操作 同样 也 可 以 使 用 对 应 的 ROS 接口 


6.3 ”运动 控制 


与 机 器 人 关节 直接 相关 的 行为 在 Roban 机 器 人 中 都 由 BodyHub 节点 所 实现 ,BodyHub 节点 
是 上 位 机 其 他 节点 与 下 位 机 通信 的 中 间 节 点 ， 机 器 人 的 所 有 控制 指令 的 发 送 和 机 器 人 数据 的 获 
取 都 通过 此 节点 实施 。BodyHub 实现 了 一 个 状态 机 来 管理 下 位 机 ， 用 BodyHub 状态 机 管理 下 位 
机 可 确保 如 能 机 这 类 执行 器 设备 ， 在 同一 时 刻 只 受 一 个 上 层 节点 控制 ， 避 免 上 层 节 点 之 间 相 互 
干扰 导致 控制 异常 ， 其 状态 机 的 状态 跳 转 图 如 图 6.13 所 示 。 


BodyHub 运 行 状态 转换 图 


ARS 机 购 入 恢复 站 立 状态 TE 


preReady 
setStatus, 
被 二 级 区 点 占用 
Tepet 


Teset 
更 新 载 入 offset.yaml 


动作 调试 节点 调用 服务 
->DirectMethod 


directOperate 


MIRER H (Diroct Method) 


缓冲 区 空 
motoQueue.empty & 
headCtriQueue.empty 


图 6.13 BodyHub 状态 转换 图 


BodyHub 有 许多 不 同 状态 ， 比 如 ready 状态 下 在 接收 到 舵 机 命令 序列 后 就 会 切换 到 running 
状态 ,此 时 在 缓冲 区 中 的 数据 会 被 机 器 人 以 10ms 的 运动 周期 下 发 , 在 运行 结束 之 后 会 自动 切换 
到 pause KAS, 通过 ROS 消息 即 可 使 得 能 机 按 实际 需要 运动 。 此 外 该 节点 还 提供 directmode， 可 
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以 用 于 对 机 器 人 的 关节 参数 进行 配置 ， 也 可 以 直接 操作 机 器 人 上 的 单个 能 机 运行 。 
6.3.1 ” 舵 机 参数 设置 


与 通常 机 器 人 关节 相 类 似 ，Roban 机 器 人 的 伺服 关节 采用 PID 控制 器 对 关节 进行 控制 ，PID 
控制 器 即 为 比例 -积分 -微分 控制 ， 其 对 应 的 数学 表达 式 为 


de(t) 
dt 


u(t) = Kye(t) + K| e (t^) dt + Ka (6.1) 


t 
0 

PID 控制 器 中 有 3 个 参数 ， 通 过 调整 这 3 个 参数 可 以 得 到 不 同 的 控制 效果 ， 其 具体 的 含义 
如 下 : | 
CD 比例 CP) 控制 。 比 例 控制 是 一 种 最 简单 的 控制 方式 。 其 控制 器 的 输出 与 输入 误差 信号 
成 比例 关系 。 当 仅 有 比例 控制 时 系统 输出 存在 稳 态 误差 。 

(2) 积分 CD 控制 。 在 积分 控制 中 ， 控 制 器 的 输出 与 输入 误差 信号 的 积分 成 正比 关系 。 对 
一 个 自动 控制 系统 ， 如 果 在 进入 稳 态 后 存在 稳 态 误差 ， 则 称 这 个 控制 系统 是 有 稳 态 误差 的 或 简 
称 有 差 系统 。 为 了 消除 稳 态 误差 ， 在 控制 器 中 必须 引入 “积分 项 ”。 积 分 项 对 误差 取决 于 时 间 的 
积分 ， 随 着 时 间 的 增加 ; 积分 项 会 增 大 。 这 样 ， 即 便 误差 很 小 , 积分 项 也 会 随 着 时 间 的 增加 而 加 
大 ， 它 推动 控制 器 的 输出 增 大 使 稳 态 误差 进一步 减 小 ， 直 到 接近 于 零 。 因 此 ， 比 例 ++ 积 分 CPD 
控制 器 ， 可 以 使 系统 在 进入 稳 态 后 几乎 无 稳 态 误差 。 

(3) 微分 (D) 控制 。 在 微分 控制 中 ， 控 制 器 的 输出 与 输入 误差 信号 的 微分 〈 即 误差 的 变化 
K) 成 正比 关系 。 自 动 控制 系统 在 克服 误差 的 调节 过 程 中 可 能 会 出 现 振 荡 甚 至 失 稳 。 其 原因 是 
存在 有 较 大 惯性 组 件 〈 环 节 ) 或 有 滞后 组 件 ， 有 具有 抑制 误差 的 作用 ， 其 变化 总 是 落后 于 误差 的 
变化 。 解 决 的 办 法 是 使 抑制 误差 的 作用 的 变化 “超前 ” 即 在 误差 接近 零 时 ， 抑 制 误差 的 作用 就 
应 该 是 零 。 这 就 是 说 ， 在 控制 器 中 仅 引 入 “比例 ”项 往往 是 不 够 的 ， 比 例 项 的 作用 仅 是 放大 误 
差 的 幅 值 ， 而 需要 增加 的 是 “微分 项 ”， 它 能 预测 误差 变化 的 趋势 ， 这 样 ， 具 有 比例 + 微分 的 控 
制 器 ， 就 能 够 提前 使 抑制 误差 的 控制 作用 等 于 零 ， 甚 至 为 负 值 ， 从 而 避免 了 被 控 量 的 严重 超 调 。 
所 以 对 有 较 大 惯性 或 滞后 的 被 控 对 象 ， 比 例 + 微分 PD) 控制 器 能 改善 系统 在 调节 过 程 中 的 动 
态 特性 。 对 于 机 器 人 的 动作 ， 需 要 根据 实际 的 动作 运行 情况 对 于 PID 参数 进行 适当 修改 。 

对 于 机 器 人 的 关节 ， 可 以 采用 /MediumSize/BodyHub/DirectMethod/InstWriteVal (bodyhub:: 
SrvInstWrite〉 的 接口 写 入 关节 的 PID 参数 ， 从 而 对 机 器 人 关节 进行 控制 。 


6.3.2 ”关节 位 置 控制 


在 机 器 人 的 使 用 过 程 中 ， 也 可 以 采用 对 单 舵 机 控制 的 方式 对 机 器 人 的 各 关节 进行 控制 ， 通 
过 对 于 系统 部 分 状态 机 跳 转 的 控制 以 及 通过 ROS 对 应 话题 数据 的 发 送 即 可 实现 对 实际 机 器 人 
关节 的 控制 操作 。 


化 ， 并 且 发 送出 去 。 

] 

2 | def set_head_servo(angles) : 

3 """set head servos angle 

4 

5 :param angles:[pan, tilt] 

6 :return: 

1 "nn 

8 angles = array.array("d", angles) 

9 Face.HeadJointPub.publish(positions-angles, mainControlID-2) 
10 time.sleep(0.01) 

1 

12 |def . init. (self): 

13 self.running - True 

14 self.size - 0.5 

15 self.face = 0, 0, 0, 0 

16 self.face_roi = 0, 0, 0, 0 

17 self.face_template = None 

18 self.found face = False 

19 self.template matching running = False 
20 self.template matching start_time = 0 
21 self.template matching current time = 0 
22 self.center x - 160 
23 self.center y = 120 

24 self.pan - 0 

25 self.tlt = 0 

26 self.error_pan = 

27 self.error_tlt = 

28 self .HeadJointPub = rospy.Publisher('MediumSize/BodyHub/HeadPosition', 
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此 处 以 人 脸 跟踪 效果 为 例 来 说 明 关 节 跟 踪 的 方式 , 截取 人 脸 跟踪 部 分 头 部 舵 机 数据 发 送 的 
部 分 代码 进行 讲解 。 
(1) 其 中 在 初始 化 部 分 对 应 的 头 部 位 置 发 送 话题 为 /MediumSizeBodyHubHeadPosition。 

(2) 在 set head servo 函数 中 实际 上 就 是 将 头 部 舵 机 对 应 的 上 下 转角 与 左右 转角 进行 格式 


JointControlPoint, queue_size=100) 
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def thread_set_servos(): 


set_head_servo([Face.pan, Face.tlt]) 
step = 0.01 
while Face.running: 
if abs(Face.error pan) > 15 or abs(Face.error tlt) > 15: 
if abs(Face.error. pan) > 15: 
Face.pan += step * Face.error, pan 
if abs(Face.error tlt) > 15: 
Face.tlt *- step * Face.error tlt 
if Face.pan » 90.0: 
Face.pan - 90.0 
if Face.pan « -90.0: 
Face.pan = -90.0 
if Face.tlt > 45.0: 
Face.tlt - 45.0 
if Face.tlt « -45.0: 
Face.tlt - -45.0 
set head servo([Face.pan, -Face.tlt]) 
else: 
time.sleep(0.01) 
def main(): 
try: ; 
rospy.init node("face tracking", anonymous-True) 
rospy.sleep(0.2) 
client. controller.send, video status(True, "/camera/label/image. raw" ,width=640, 
height-480) 
image topic = "/camera/color/image. raw" 
rospy.Subscriber(image topic, Image, image callback) 
rospy.Subscriber('terminate current process', String, terminate) 
async. do, job(detectFace) 
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63 async_do_job(thread_face_center) 

64 async_do_job(thread_set_servos) 

65 while Face.running: 

66 time.sleep(0.01) 

67 

68 | except Exception as err: 

69 serror (err) 

70 | finally: 

71 client controller.send video status(False, "/camera/label/image raw",width 
=640, height=480) 

72 SERVO .HeadJointTransfer( [0,0] ,time=1000) 

73 SERVO .MotoWait () 

74 finishsend() 

75 

76 |if _mame__ == '__main__' 

77 main() 


6.3.3 ” 步 态 控制 


BodyHub 节点 《也 可 称 为 功能 包 ) 也 可 以 提供 用 于 Roban 机 器 人 的 行走 控制 ， 给 出 脚印 接 
口 用 于 控制 机 器 人 的 行走 。Roban 的 行走 控制 使 用 “线性 倒立 摆 ” 模型， 在 每 个 周期 中 采集 来 自 
传感器 的 实际 关节 位 置信 号 ， 与 位 移 〈 即 位 置 ) 和 身体 的 倾斜 角度 等 期 望 值 进行 比较 后 ， 利 用 
控制 算法 计算 出 控制 量 ， 驱 动 电机 实现 对 关节 的 实时 控制 。 行 走 控制 的 目标 是 尽快 达到 一 个 平 
衡 位 置 ， 并 且 没有 大 的 振荡 和 过 大 的 角度 和 速度 ， 当 到 达 期 望 的 位 置 后 ， 采 用 减速 的 办 法 组 组 
站 立 。 

每 步 包 括 双 腿 支撑 和 单 腿 支撑 两 个 阶段 。 其 中 ， 双 腿 支 撑 时 间 约 占 10%， 每 步 的 行走 距离 
可 调 , 最 大 可 达到 8cm 。 脚 运动 轨迹 是 一 条 平滑 曲线 ， 如 图 6.14 所 示 。 图 中 的 平滑 曲线 , 是 在 第 
卡 儿 空间 〈 包 括 m. y. z 轴 ) 中 , 利用 初始 速度 和 关键 点 , 采用 五 次 样 条 曲线 插值 方法 进行 规划 ， 
这 样 的 规划 可 以 使 得 脚 部 在 沿 该 曲线 运动 时 做 到 平滑 连续 ， 脚 步 运行 轨迹 示意 如 图 6.14 所 示 。 

给 定 的 每 个 落脚 点 的 参考 位 置 使 用 一 个 三 维 向 量 〈Vector3D) 来 定义 ，3 个 数值 分 别 为 
(dz, dy, theta)。 如 图 6.15 所 示 ， 在 描述 左 脚 位 置 时 ， 以 右 脚 为 参照 点 dz 和 dy 分 别 为 左 脚 在 
z 和 yy 方向 上 与 参考 点 之 间 的 距离 ，theta ASE z 轴 旋 转 的 角度 ， 即 左 转 或 右 转 的 角度 。 对 于 行 
走 过 程 中 的 行走 周期 、 最 大 高 度 , 系统 会 自动 采用 默认 值 执行 。 


212 41 仿 人 机 器 人 建 模 与 控制 


A 
e 
=e 
EE 


D 
t 

---4-- 
D 


615 ”脚步 参数 定义 


下 面 以 一 个 具体 例子 来 讲解 如 何 控 制 机 器 人 步 态 行走 : 


#!/usr/bin/env Python 

import rospy 

import time 

from bodyhub.srv import SrvState 

from std_msgs.msg import Bool 

from std_msgs.msg import Float64MultiArray 

GAIT RANGE - 0.05 

walkingPub = rospy.Publisher('/gaitCommand', Float64MultiArray, queue. size-1) 


Li 


DPI、 了 fk WN 一 


_ 
c 


def walking client(walkstate): 


_ 
一 


rospy.wait for service("/MediumSize/BodyHub/StateJump") 


_ 
N 


client = rospy.ServiceProxy("/MediumSize/BodyHub/StateJump", SrvState) 
client(2, walkstate) 


jà — j 
vA e Ww 


def slow walk(direction, stepnum): 
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16 de 
17 :param direction: "forward" or "backward" 
18 :param stepnum: int num 
19 :return: 
20 nw 
21 array - [0.0, 0.0, 0.0] 
22 if direction -- "forward": 
23 array[0] = GAIT RANGE 
24 elif direction -- "backward": 
25 array[0] = -1 * GAIT RANGE 
26 else: 
27 rospy.logerr("error, walk, direction") 
28 for i in range(stepnum): 
29 if rospy.wait for message("/requestGaitCommand", Bool): 
30 walkingPub.publish(data-array) 
31 
32 |def main(): 
33 rospy.init_node("gait_test",) 
34 time.sleep(2) 
35 walking client("setStatus") 
36 walking_client ("walking") 
37 slow_walk("forward" ,6) 
38 
39 | if _name__ == '__main__' 
40 main() 
例子 中 如 第 2 行 和 第 3 4T 
import rospy 


import time 


分 别 引入 的 ROS 的 Python 库 用 于 ROS 相关 操作 ， 也 引入 了 time 库 用 于 延 时 。 


例子 中 的 第 4~6 行 引入 了 步 态 操作 所 需 的 服务 与 消息 : 


from bodyhub.srv import SrvState 
from std_msgs.msg import Bool 


from std msgs.msg import Float64MultiArray 
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例子 中 的 第 8 行 初始 化 了 一 个 用 于 脚印 发 送 的 publish: 


walkingPub = rospy.Publisher('/gaitCommand', Float64MultiArray, queue_size=1) 


例子 中 的 第 10~13 行 实现 了 一 个 BodyHub 初始 化 函数 步 态 状态 机 的 控制 函数 ， 通 过 这 个 
函数 可 以 方便 地 控制 机 器 人 BodyHub 节点 的 状态 跳 转 。 


def walking client(walkstate): | 
rospy.wait for service("/MediumSize/BodyHub/StateJump") | 
client = rospy.ServiceProxy("/MediumSize/BodyHub/StateJump", SrvState) 
client (2, walkstate) 


例子 中 的 第 15~31 行 实现 了 一 个 步 态 运行 的 函数 ， 其 包括 两 个 参数 ， 分 别 为 前 进 与 后 退 的 
指令 和 已 经 前 进 与 后 退 的 步 数 ， 注 意 到 其 中 有 对 于 /requestGaitCommand 这 个 话题 的 订阅 ,表示 
只 有 在 步 态 节点 需要 接收 脚印 数据 时 才 会 对 脚印 数据 进行 发 送 。 


def slow_walk(direction, stepnum): 
LER 


:param direction: "forward" or "backward" 


:param stepnum: int num 
:return: 


array = [0.0, 0.0, 0.0] 


if direction -- "forward": 
array[0] = GAIT RANGE 
elif direction -- "backward": 


array[0] = -1 * GAIT_RANGE 
else: 
rospy.logerr("error, walk direction") 
for i in range(stepnum): 
if rospy.wait, for message("/requestGaitCommand", Bool): 
walkingPub.publish(data-array) 


最 后 是 main() 函数 部 分 : 在 开始 部 分 先 初始 化 了 gait test 这 个 ROS 节点 ， 然 后 为 了 等 待 节点 
的 初始 化 完成 延 时 了 2s， 之 后 调用 状态 跳 转 相关 的 函数 ， 先 获取 actexec 这 个 包 的 控制 权限 ， 然 后 
调用 slowwalk() 函数 向 BodyHub 节点 发 布 对 应 数据 ， 发 出 指令 让 机 器 人 前 进 6 步 。 代 码 如 下 : 


def main(): 
rospy.init node("gait test",) 


NC 218 


time.sleep(2) 
walking client ("setStatus") 
walking client ("walking") 


slow walk("forward",6) 


儿 种 常见 的 步 态 运动 方式 的 参数 设置 如 下 : 
前 进 : dx>0,dy=0,theta=0; 
Jai: dx<0,dy=0,theta=0; 
左 移 : dx=0,dy>0,theta=0; 
di: dx=0,dy<0,theta=0. 


6.4 运动 学 正解 


ik module 节点 包含 运动 学 的 正 逆 解 实现 ， 其 运行 依赖 BodyHub 节点 。 


6.4.1 ”运行 IK 节点 


PUM ik module 节点 ， 放 到 ROS 工作 空间 中 ,确保 工作 空间 中 包含 BodyHub 节点 ， 编 译 工 
作 空 间 。 

确保 BodyHub 节点 正在 运行 ， 若 未 运行 ， 则 启动 BodyHub 节点 。 

打开 终端 运行 指令 rosrun ik module ik module node, JAZ) IK 节点 。 


6.4.2 ”计算 四 胶 未 端 位 置 


向 MediumSize/IKmodule/GetPoses 服务 发 送 请 求 ， 即 可 获得 根据 机 器 人 当前 关节 正解 得 到 
的 四 胶 的 末端 位 置 。 

获取 的 末端 位 置 如 图 6.16 所 示 。 

图 6.16 中 的 position AMLE; orientation 为 姿态 ， 从 上 到 下 分 别 为 机 器 人 当前 的 左 腿 位 姿 、 
右 腿 位 姿 、 左 手 位 姿 、 右 手 位 姿 。 

四 有 上肢 姿态 的 获取 使 用 了 运动 学 正解 ， 根 据 关 节 角 度 进 行 运动 学 正解 的 代码 如 下 : 


bool syncIkModlePose() 

{ 
Eigen: :VectorXd servoValueVector; // 舵 机 实时 关节 角度 
std::vector«double, t? ikModelJoint; 
if (!getAngleOfJoint(ikModelJoint)) // 获取 机 器 人 当前 关节 角度 
1 


216 4l 仿 人 机 器 人 建 模 与 控制 


ROS_ERROR("getAngleOfJoint error!"); 
return false; 
} 
servoValueVector.resize(ikModelJoint.size()); 
for (uinti6 t i = 0; i < ikModelJoint.size(); i++) 
{ 
servoValueVector[i] = ikModelJoint [i]; 
} 
mWalk.talosRobot.talosmbc.q = sVectorToParam(mWalk.talosRobot.talos, 
servoValueVector.segment(0; 18) * Util::TO_RADIAN) ; 
rbd::forwardKinematics(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc); // 运动 


// 学 正解 计算 四 肢 末 端 位 置 


return true; 


6.16 四肢 末 端 位 置 
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代码 基本 流程 : 

(1) 通过 getAngleOfJoint 函数 获取 机 器 人 当前 关节 的 角度 。 

(2) 将 关节 角度 赋值 给 逆 解 模型 。 

(3) rbd::forwardKinematics 函数 根据 关节 角度 正解 计算 模型 〈 即 机 器 人 ) 四 肢 术 端 位 置 。 
函数 可 获取 机 器 人 当前 关节 角度 并 计算 出 位 姿 。 

获取 机 器 人 位 姿 的 ROS 服务 回调 函数 : 


bool GetPosesCallback(ik module::SrvPoses::Request &req, 


ik module::SrvPoses::Response &res) 


if (syncIkModlePose()) // 同步 机 器 人 位 姿 
i 
geometry msgs::Pose poseMsg; 
Eigen::Matrix4d leftFootPose, rightFootPose, leftHandPose, rightHandPose, pose; 
std: :queue<Eigen: :Matrix4d> poseHomogeneousQueue; 
Eigen: :Matrix3d rotation; 
Eigen: :Vector3d translation; 


Eigen: :Quaterniond q; 


leftFootPose = sva::conversions: : toHomogeneous (mWalk.talosRobot.talosmbc.bodyPosW 


(71); 

rightFootPose = sva::conversions: :toHomogeneous (mWalk.talosRobot.talosmbc. 
bodyPosW[14]) ; 

leftHandPose = sva::conversions: : toHomogeneous (mWalk.talosRobot.talosmbc.bodyPosW 
[18] ) ; 

rightHandPose = sva::conversions: : toHomogeneous (mWalk.talosRobot.talosmbc. 
bodyPosW[22]) ; 


poseHomogeneousQueue . push (leftFootPose) ; 
poseHomogeneousQueue. push(rightFootPose) ; 
poseHomogeneousQueue . push(leftHandPose) ; 


poseHomogeneousQueue. push (rightHandPose) ; 


res.poses.clear() ; 


while (ros::ok() && !poseHomogeneousQueue.empty()) // 转换 为 ROS 中 的 geometry_msgs/ 
// Pose 格式 


218 <4 仿 人 机 器 人 建 模 与 控制 


pose = poseHomogeneousQueue.front(); 
poseHomogeneousQueue . pop O) ; 

rotation = pose.block«3, 3>(0, 0).transpose(); 
translation = pose.block<3, 1>(0, 3); 


q = Eigen::Quaterniond(rotation); 


poseMsg.position.x = translation[0]; 
poseMsg.position.y = translation[1]; 


poseMsg.position.z = translation[2]; 


poseMsg.orientation.w = q.w(); 
poseMsg.orientation.x = q.x(); 
poseMsg.orientation.y = q.y(); 


poseMsg.orientation.z = q.z(); 


res.poses.push_back(poseMsg) ; 
} 
return true; 
} 
return false; 


F 


代码 基本 流程 : 

(1) 调用 前 面 介绍 的 syncIkModlePose 函数 同步 机 器 人 模型 位 姿 。 

(2) 将 位 姿 信 息 进行 坐标 变换 ， 之 后 将 数据 转换 为 ROS 中 的 geometry_msgs/Pose 格式 。 
(3) 应 答 转 换 后 的 位 姿 数 据 。 


6.5 ”运动 学 逆 解 


65.1 HAE 


ik module 功能 包 中 包含 控制 机 器 人 扭 腰 的 示例 程序 ， 在 6.4.2 T5817 IK 节点 的 基础 上 ,可 
直接 运行 机 器 人 扭 腰 程序 。 
打开 终端 ， 执 行 如 下 命令 : 


rosrun ik_module ik_module_yawaround.py 
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之 后 ， 机 器 人 会 执行 扭 腰 动 作 ， 如 图 6.17 所 示 。 
机 器 人 捏 腰 程 序 如 下 ， 


def main(self): 
if self.toInitPoses(): // 运行 到 初始 位 姿 
self.getposes() // 获取 机 器 人 的 位 姿 信息 
HEHHHHHHHHEAEHHHHHBHHH BAHAR HHH HHH HHH HH HH HHH SH 
i, yawCount = 0, 0 
count = 400 
# 手臂 摆动 的 角度 ,中 心 坐标 ,相对 半径 
downAngle = 70*math.pi/180.0 # in radians 
yawAngle = 70*math.pi/180.0 # in radians 
centerX, centerY, centerZ = Shoulder2 X, Shoulder2 Y, Shoulder2_Z 
downRadius = Shoulderi_Y+Elbow1_Y+Wristi_Y-1e-6 
yawRadius = downRadius*math.sin(downAngle) 
# torso parameters 
torsoHeight = -self.leftLegZ 
torsoHeightWalk = torsoHeight 
SERRA EESEREREHEAHENHASHER PPE RE NHARE R ESRB ERE EHHH HH 
# loop 
while not rospy.is_shutdown(): 
i t= 1 
self .PosPara_wF.Torso_x = 0.0 
self .PosPara_wF.Torso_y = 0.0 
self .PosPara_wF.Torso_z = torsoHeightWalk 
self .PosPara_wF.Lfoot_y = self.leftLegY 
self.PosPara_wF.Rfoot_y = self.rightLegY 


self .PosPara_wF.Torso_Y = math.pi/4.5*math.sin(2*math.pi*i/count) 


if yawCount==0: 

# put hands down 

self.leftArmY = centerY + downRadius*math.cos( i*downAngle/count ) 
self.rightArmY = -centerY - downRadius*math.cos( i*downAngle/count ) 
self.leftArmZ = centerZ - downRadius*math.sin( i*downAngle/count ) 
self.rightArmZ = centerZ - downRadius*math.sin( i*downAngle/count ) 
# print(yawCount,i,self.leftArmX,self.leftArmY,self.leftArmZ,self. 

rightArmX,self.rightArmY,self.rightArmZ) 


220 «4 仿 人 机 器 人 建 模 与 控制 


elif yawCount==1 or yawCount==2: 
# yaw hans around 
swingAngle = yawAngle*math.sin(2*math.pi/count*i) 
self.leftArmX = yawRadius*math.sin(swingAngle) 
self.rightArmX = -yawRadius*math.sin(swingAngle) 
self.leftArmY = centerY + downRadius*math.cos(downAngle) 
self .rightArmY = -centerY - downRadius*math.cos(downAngle) 
self.leftArmZ = centerZ - yawRadius*math.cos(swingAngle) 
self.rightArmZ = centerZ - yawRadius*math.cos(swingAngle) 
# print (yawCount ,i,self.leftArmX,self.leftArmY,self.leftArmZ,self. 

rightArmX,self.rightArmY,self.rightArmZ) 


elif yawCount--3: 

# put hands up 

self.leftArmY = centerY + downRadius*math.cos( (count-i)*downAngle/ 
count ) 

self.rightArmY - -centerY - downRadius*math.cos( (count-i)*downAngle/ 
count ) 

self.leftArmZ - centerZ - downRadius*math.sin( (count-i)*downAngle/ 
count ) 

self.rightArmZ - centerZ - downRadius*math.sin( (count-i)*downAngle/ 
count ) 

# print(yawCount,i,self.leftArmX,self.leftArmY,self.leftArmZ,self. 
rightArmX,self.rightArmY,self.rightArm2Z) 


elif yawCount»-4: 
i, yawCount - 0, O 
rospy.sleep(1) 
rospy.loginfo("waitPostureDone...") 
self.waitPostureDone() 
rospy.sleep(1) 
rospy.loginfo("Yaw around done.") 
self.reset() 


return True 
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if i >= count: 
i= 1 


yawCount += 1 


poseArrayMsg = PoseArray() 

# Left Leg 

leftLegPosMsg = self.getleftlegPosMsg() 
poseArrayMsg . poses. append (leftLegPosMsg) 
# Right Leg 

rightLegPosMsg = self.getrightlegPosMsg() 
poseArrayMsg. poses .append(rightLegPosMsg) 
# Left Arm 

leftArmPosMsg = self.getleftarmPosMsg() 
poseArrayMsg. poses. append (leftArmPosMsg) 
# Right Arm 

rightArmPosMsg = self.getrightarmPosMsg() 
poseArrayMsg. poses, append(rightArmPosMsg) 


# Publish target Poses 
poseArrayMsg.controlld = 6 


poseArrayMsg.poses-[] 


self.targetPosesPub.publish(poseArrayMsg) 


Hh, yawCount 40, 3 时 ， 给 定 机 器 人 放下 手 、 抬 起 手 的 位 置 ; yawCount X 1. 2 时 ,给 
定 机 器 人 前 、 后 摆手 的 位 置 ，yawCount 为 4 时， 等 待 扭 腰 结 束 后 复位 机 器 人 。 


放下 手 、 抬 起 手 以 及 摆手 都 属于 圆 弧 运动 ， 根 据 手臂 摆动 的 角度 、 中 心 坐标 和 相对 半径 决 


定 。 坐 标 和 半径 设置 代码 如 下 ; 


# 手臂 摆动 的 角度 ,中 心 坐标 ,相对 半径 
downAngle = 70*math.pi/180.0 # in radians 
yawAngle = 70*math.pi/180.0 # in radians 


yawRadius = downRadius*math.sin(downAngle) 


centerX, centerY, centerZ = Shoulder2_X, Shoulder2_Y, Shoulder2_Z 
downRadius = Shoulder1_Y+Elbow1_Y+Wrist1_Y-1e-6 
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6.17 机 器 人 扭 腰 示 意图 


扭 腰 的 基本 原理 是 给 定 腰部 位 姿 ， 计 算 双 脚 的 相对 位 姿 ， 达 到 控制 机 器 人 腰部 运动 的 目的 ， 
以 下 代码 给 定 机 器 人 身体 扭 动 的 位 姿 : 


self.PosPara wF.Torso x = 0.0 
self.PosPara wF.Torso y - 0.0 
self.PosPara wF.Torso z = torsoHeightWalk 
self.PosPara wF.Lfoot y = self.leftLegY 
self.PosPara wF.Rfoot y - self.rightLegY 


self.PosPara wF.Torso Y = math.pi/4.5*math.sin(2*math.pi*i/count) 


给 定 机 器 人 四 肢 位 姿 后 ， 通 过 MediumSize/IKmodule/TargetPoses 话题 发 送 给 IK 节点 ， 发 送 
代码 如 下 : 


poseArrayMsg = PoseArray() 

# Left Leg 

leftLegPosMsg = self.getleftlegPosMsg() 
poseArrayMsg.poses.append(leftLegPosMsg) 
# Right Leg 

rightLegPosMsg = self.getrightlegPosMsg() 
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poseArrayMsg. poses. append (rightLegPosMsg) 
# Left Arm 

leftArmPosMsg = self.getleftarmPosMsg() 
poseArrayMsg. poses. append (leftArmPosMsg) 
# Right Arm 

rightArmPosMsg = self.getrightarmPosMsg() 
poseArrayMsg. poses. append (rightArmPosMsg) 
# Publish target Poses 
poseArrayMsg.controlld = 6 
self.targetPosesPub. publish (poseArrayMsg) 
poseArrayMsg.poses-[] 


6.5.2 HEP IK 逆 解 的 处 理 


目标 位 姿 MediumSize/IKmodule/TargetPoses 话题 的 回调 函数 ; 


void TargetPosesCallback(const ik_module::PoseArray::ConstPtr &msg) 
1 

sva::PTransformd targetPos; 

std: :queue<sva::PTransformd> targetQueue; 


std: : vector<geometry_msgs::Pose> posesArr; 


if (msg-»controlId == currentControlId) 
{ 
// ROS_INFO("Received new posearray with ID Ad", msg->controlId) ; 


posesArr = msg->poses; 


for (uint8 t i = 0; i < posesArr.size(); i++) 
1 
Eigen::Quaterniond q(posesArr[il.orientation.w, posesArr[i].orientation.x, 
posesArr[i].orientation.y, posesArr[i].orientation.z); 
targetPos.rotation() = q.toRotationMatrix() ; 
targetPos.translation() << posesArr[i].position.x, posesArr[i].position.y, 
posesArr[i].position.z; 


targetQueue.push(targetPos) ; 
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pthread_mutex_lock(&mtxPQ) ; 
posturesQueue. push(targetQueue) ; 
pthread mutex unlock(&mtxPQ); 


// 数据 到 达 
if ((ikmoduleState == StateEnum::ready) || 
(ikmoduleState 
UpdateState (StateEnum: : running) ; 


= StateEnum: : pause) ) 


} 
else 
{ 
ROS_ERROR("IkModule is busy with controlID 4d", currentControlld); 
} 


回调 函数 将 收 到 的 位 姿 转 换 为 sva::PTransformd 类 型 并 存放 到 posturesQueue 中 。 
IK 线程 : 


void ikThread() 
{ 
ROS_INFO("IkThread initialized!"); 


std::vector<double> jointValueVector; 

std: :queue<sva::PTransformd> targetPosesQueue; 

sva::PTransformd legLeftPos, legRightPos, armLeftPos, armRightPos; 

rbd::InverseKinematics leftLegIk(mWalk.talosRobot.talos, mWalk.talosRobot.talos. 
bodyIndexByName("leftLegLinkSole")); 

rbd::InverseKinematics rightLegIk(mWalk.talosRobot.talos, mWalk.talosRobot.talos. 
bodyIndexByName("rightLegLinkSole")); 

rbd::InverseKinematics leftArmIk(mWalk.talosRobot.talos, mWalk.talosRobot.talos. 
bodyIndexByName ("leftArmLinkSole")) ; 

rbd::InverseKinematics rightArmIk(mWalk.talosRobot.talos, mWalk.talosRobot.talos. 
bodyIndexByName("rightArmLinkSole")) ; 


mWalk.talosRobot.talosmbc.zero(mWalk.talosRobot.talos); 
rbd::forwardKinematics(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc); 
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// Eigen::Matrix«double, 18, 1» theta; 
// theta €«4:0,:05--10,:30, —10;,.0; 0,.0,. 510, 30, -105,.04-04 0, 0540, 0, 0; 
// mWalk.talosRobot.talosmbc.q = sVectorToParam(mWalk.talosRobot.talos, theta. 
segment (0,18) *Util::TO_RADIAN) ; 
// rbd::forwardKinematics(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc) ; 
while (ros::ok()) 
{ 
// 求 运动 学 逆 解 
if (!posturesQueue.empty()) 
£ 
pthread_mutex_lock(&mtxPQ) ; 
targetPosesQueue = posturesQueue.front() ; 
posturesQueue. pop() ; 


pthread, mutex unlock(&mtxPQ); 


legLeftPos = targetPosesQueue.front(); 

targetPosesQueue.pop() ; 

if (leftLegIk.inverseKinematics(mWalk.talosRobot.talos, mWalk.talosRobot. 
talosmbc, legLeftPos)) 


mWalk.jointValue.segment(mWalk.LLEG JOINT START, mWalk.LLEG, JOINT, NUM) = 
sParamToVector(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc.q). 
segment(0, 6) * Util::TO DEGREE; 
i 


else 


legRightPos = targetPosesQueue.front(); 

targetPosesQueue.pop(); 

if (rightLegIk.inverseKinematics(mWalk.talosRobot.talos, mWalk.talosRobot. 

talosmbc, legRightPos)) 
1 
mWalk.jointValue.segment(mWalk.RLEG, JOINT. START, mWalk.RLEG. JOINT. NUM) = 

sParamToVector(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc.q). 
segment(6, 6) * Util::TO, DEGREE; 
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} 


else 


armLeftPos = targetPosesQueue.front(); 

targetPosesQueue.pop(); 

if (leftArmIk.inverseKinematics(mWalk.talosRobot.talos, mWalk.talosRobot. 

talosmbc, armLeftPos, 3)) 
1 
mWalk.jointValue.segment(mWalk.LARM, JOINT. START, mWalk.LARM, JOINT NUM) = 

sParamToVector(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc.q). 
segment(12, 3) * Util::TO, DEGREE; 

1; 


else 


armRightPos = targetPosesQueue.front(); 

targetPosesQueue.popO; 

if (rightArmIk.inverseKinematics(mWalk.talosRobot.talos, mWalk.talosRobot. 
talosmbc, armRightPos, 3)) 


mWalk.jointValue.segment(mWalk.RARM. JOINT. START, mWalk.RARM JOINT NUM) = 
sParamToVector(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc.q). 
segment(15, 3) * Util::TO, DEGREE; 
} 


else 


jointValueVector.resize(mWalk.jointValue.size()); 
for (uint8_t i = 0; i < 12; it+) 


jointValueVector[i] = mWalk.jointValue[i] * jointDirection[i]; 


for (uint8 t i = 12; i < 15; i++) 
jointValueVector[i] = mWalk.jointValue[i + 3] * jointDirection[il; 
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for (uint8 t i = 15; i < 18; i++) 


jointValueVector[i] = mWalk.jointValue[i - 3] * jointDirection[i]; 


pthread mutex lock(&mtxJVQ); 
jointValuesQueue.push(jointValueVector); 
pthread mutex unlock(&mtxJVQ); 


mWalk.talosRobot.talosmbc.q = sVectorToParam(mWalk.talosRobot.talos, mWalk. 
jointValue.segment(0, 18) * Util::TO RADIAN); 
rbd::forwardKinematics(mWalk.talosRobot.talos, mWalk.talosRobot.talosmbc); 


IK 线程 的 基本 流程 : 

(1) 循环 判断 posturesQueue 是 否 为 空 ， 若 不 为 空 ， 执 行 第 (2) Gb. 

C2) 获取 其 中 机 器 人 四 肢 位 姿 的 一 个 数据 ， 分 别 对 四 肢 进行 逆 解 。 

(3) 将 根据 逆 解 计算 得 出 的 关节 角度 存放 到 jointValuesQueue 队列 中 , 在 发 布线 程 中 发 送 给 
机 器 人 执行 。 

(4) 继续 执行 第 (1) 步 。 

通过 以 上 过 程 , 指定 机 器 人 位 盗 , IK 节点 道 解 得 到 关节 角度 , 将 角度 发 送 给 机 器 人 执行 , K 
现 机 器 人 扭 腰 动 作 。 


6.5.3 diss A Sc 


ik module 功能 包 中 包含 控制 机 器 人 晃 腰 的 示例 程序 ， 在 6.5.2 节 运 行 IK 节点 的 基础 上 ,可 
直接 运行 机 器 人 扭 腰 程 序 。 
打开 终端 ， 执 行 如 下 命令 : 


rosrun ik_module ik_module_swingaround.py 


之 后 ， 机 器 人 会 执行 晃 腰 动作 ， 如 图 6.18 所 示 。 
晃 腰 程序 结构 与 扭 腰 程 序 相似 ， 机 器 人 晃 腰 程序 如 下 : 


def main(self): 


if self.toInitPoses(): 


self.getposes() 
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poseArrayMsg = PoseArray() 

# torso parameters 

torsoHeight = -self.leftLegZ 

torsoHeightWalk = torsoHeight 

t = math.sqrt (torsoHeightWalk*torsoHeightWalk + 0*0) 
r = 0.06 

r/(2xmath.pi) 


a 
i, rCount = 1, 3 

# 手臂 摆动 的 角度 ,中心 坐标 ,相对 半径 

count = 300 

downAngle = 70*math.pi/180.0 # in radians 

centerX, centerY, centerZ = Shoulder2_X, Shoulder2 Y, Shoulder2 Z 
downRadius = Shoulderi_Y+Elbow1_Y+Wristi_Y-1e-6 


# loop 
while not rospy.is shutdown(): 
if i <= count: 
i += 1 


sita = 2*math.pi*i/count 


x = a*sita*math.cos(sita) 


a*sita*math.sin(sita) 


y 
# put hands down 


self.leftArmY = centerY + downRadius*math.cos( i*downAngle/count ) 
self.rightArmY - -centerY - downRadius*math.cos( i*downAngle/count ) 
self.leftArmZ = centerZ - downRadius*math.sin( i*downAngle/count ) 
self.rightArmZ - centerZ - downRadius*math.sin( i*downAngle/count ) 


elif i <= count*(rCount-*i): 
i+=1 


sita = 2*math.pi*i/count 


x = r*math.cos(sita) 


y = r*math.sin(sita) 


elif i <= count*(rCount*2): 
i += 1 
sita = -2*math.pi*(count*(rCount+2)-i)/count 


x = a*sita*math.cos(sita*math.pi) 
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y = a*sita*math.sin(sita+math. pi) 


elif i > count*(rCount*2): 
i= 1; #reset i 
rospy.sleep(1) 
rospy.loginfo("waitPostureDone...") 
self.waitPostureDone() 
rospy.sleep(1) í 
rospy.loginfo("Swing around done.") 
self.reset() 


return True 


t_V = math.sqrt(t*t-x*x-y*y) 

# update torso parameters 

self .PosPara_wF.Torso_x = 0 

self.PosPara wF.Torso y = 0 

self.PosPara wF.Torso z - torsoHeightWalk 

self.PosPara wF.Torso R = math.asin(y/t. V) 

self.PosPara wF.Torso P = math.asin(x/t V) 

self.PosPara wF.Torso Y = 0.0 

self.PosPara wF.Lfoot y - self.leftLegY 

self.PosPara wF.Rfoot y - self.rightLegY 

# print( "Torso Pose: ", i, self.PosPara wF.Torso x, self.PosPara wF.Torso 
_y, self.PosPara_wF.Torso_z, self.PosPara wF.Torso R, self.PosPara wF. 
Torso P,self.PosPara wF.Torso. Y) 

THHEHHEHEHHEHHHEHHHHHHHEHHHHHBHBHBHHHBHBHHHHERNH ERE 

# Left Leg 

leftLegPosMsg = self.getleftlegPosMsg() 

poseArrayMsg.poses.append(leftLegPosMsg) 

* Right Leg 

rightLegPosMsg = self.getrightlegPosMsg() 

poseArrayMsg . poses. append (rightLegPosMsg) 

# Left Arm 

leftArmPosMsg = self.getleftarmPosMsg() 

poseArrayMsg. poses. append (leftArmPosMsg) 

# Right Arm 
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rightArmPosMsg = self.getrightarmPosMsg() 
poseArrayMsg.poses.append(rightArmPosMsg) 
# Publish target Poses 
poseArrayMsg.controlld - 6 
self.targetPosesPub.publish(poseArrayMsg) 


poseArrayMsg.poses-[] 


图 6.18 HARER 


晃 腰 是 以 倒立 的 圆锥 为 参考 ， 给 定 腰 部 位 姿 ， 使 其 按照 螺旋 线 运行 ， 通 过 坐标 变换 为 脚 部 
位 次 ， 将 四 肢 和 脚 部 的 位 姿 发 送 给 I 节点 。 


6.6 ”自动 避 障 实践 


自动 避 障 实践 ， 主 要 是 实现 了 一 个 利用 深度 相机 获取 障碍 物 的 深度 ， 从 而 判断 执行 特定 步 
态 情 况 的 综合 应 用 。 


6.6.1 3D 相机 的 原理 


我 们 采用 的 相机 是 Intel RealSense D435。 该 相机 使 用 结构 光 的 方式 来 获取 深度 信息 。 

结构 光 (structured light): 通常 采用 特定 波长 的 不 可 见 的 红外 激光 作为 光源 ， 它 发 射出 来 的 
光 经 过 一 定 的 编码 投影 在 物体 上 ， 通 过 一 定 算法 计算 返回 的 编码 图 案 的 畸变 来 得 到 物体 的 位 置 
和 深度 信息 。 根 据 编 码 图 案 的 不 同 ， 一般 有 条 纹 结构 光 、 编 码 结构 光 、 散 斑 结 构 光 。3D 相机 原 
理 图 如 图 6.19 所 示 。 
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图 像 
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6.19 3D 相机 


特定 波长 的 激光 发 出 的 结构 光照 射 在 物体 表面 ， 其 反射 的 光线 被 带 滤波 的 相机 接收 ， 滤 波 
片 保证 只 有 该 波长 的 光线 能 为 相机 所 接收 。Asic 芯片 对 接收 到 的 光斑 图 像 进行 运算 ， 得 出 物体 
的 深度 数据 。 

其 基本 的 算法 原理 可 参看 图 6.20。 

散 斑 就 是 激光 照射 到 粗糙 物体 或 穿 透 毛 玻璃 后 随机 形成 的 衍射 班 点。 这些 散 斑 具 有 高 度 的 
随机 性 ， 而 且 会 随 着 距离 的 不 同 而 变换 图 案 。 

也 就 是 说 ， 空 间 中 任意 两 处 的 散 斑 图 案 都 是 不 同 的 。 只 要 在 空间 中 打上 这 样 的 结构 光 ， 整 
个 空间 就 都 被 做 了 标记 ， 把 一 个 物体 放 进 这 个 空间 ， 只 要 看 看 物体 上 面 的 散 斑 图 案 ， 就 可 以 知 
道 这 个 物体 在 什么 位 置 了 。 当 然 ， 在 这 之 前 要 把 整个 空间 的 散 斑 图 案 都 记录 下 来 ， 所 以 要 先 做 
一 次 光源 标定 ， 通 过 对 比 标定 平面 的 光斑 分 布 ， 就 能 精确 计算 出 当前 物体 距 相机 的 距离 。 


AS: 激光 投影 模块 位 置 

CH: CMOS 相 机 位 置 

d: 基线 

上 参考 面 到 相机 的 位 置 

z(z, y): 物体 表面 (z, 切 点 到 标定 平面 的 距离 
物体 表面 任 一 点 P(z, y) 的 深度 信息 可 以 通过 比较 
P(z, 与 激光 散 斑 投 射 到 参考 平面 (虚线 所 示 ) 


名 上 的 成 Ps 切 的 z 访 向 的 偏 移 量 得 到 


图 6.20 基本 原理 


6.6.2 ”设计 思路 以 及 步骤 


可 以 将 程序 分 成 如 下 几 个 步骤 
(D 初始 化 ROS 节点 。 


m « OHA AIRS 


(2) 进入 步 态 状 态 。 

C3) 获取 5 次 深度 相机 的 数据 并 取 平 均值 。 

(4) 根据 深度 均值 去 执行 相应 的 动作 。 

深度 相机 获取 所 有 深度 的 话题 名 称 为 /camera/depth/image_rect_raw, 使 用 这 个 话题 可 以 获取 
深度 相机 的 所 有 点 的 深度 ， 在 当前 的 实践 中 ， 我 们 取 了 中 心 100 x 100 像素 区 域 的 均值 。 

对 于 不 同 距离 的 判断 ， 我 们 采用 以 下 方式 进行 处 理 ， 

C1) 距离 小 于 150mm， 显 示 为 距离 太 近 ， 无 法 进行 处 理 。 

(2) 距离 小 于 250mm， 距 离 前 方 障碍 物 比 较 近 ， 需 要 往 后 退 一 点 才能 运动 。 

(3) 距离 小 于 500mm, A 4% 30°. 

(4) 距离 小 于 1000mm， 前 进 。 

(5) 距离 大 于 1000mm， 距 离 较 远 ， 前 进 可 以 走 得 快 一 些 。 


6.6.3 ”示例 代码 
示例 代码 如 下 : 


#!/usr/bin/Python 
# coding=UTF-8 


import rospy 


from cv_bridge import CvBridge 
from sensor_msgs.msg import Image 
from lejulib import * 

import numpy as np 


from motion.motionControl import * 


GAIT_RANGE = 0.05 
ROTATION_RANGE = 10.0 
ROI = (100, 100) 


def slow_walk(direction, stepnum=1, angle=None): 
nun 
:param direction: "forward" ,"backward" or "rotation" 
:Param stepnum: int num 


:return: 


array = [0.0, 0.0, 0.0] 


D RENE M 


if direction == "forward": 
array[0] = GAIT. RANGE 
elif direction == "backward": 


array[0] = -1 * GAIT_RANGE 
elif direction == "rotation": 
array[2] = ROTATION_RANGE if angle > 0 else -1 * ROTATION_RANGE 
stepnum = int(abs(angle) / ROTATION_RANGE) 
else: | 
rospy.logerr("error walk direction") 
for _ in range(stepnum) : 


SendGaitCommand(array[0], array[1], array[2]) 


def move(mean distance): 

print (mean, distance) 

if mean distance « 150: 
print(" 离 得 太 近 了 ， 我 识别 不 到 了 。") 
return 

elif mean_distance > 1000: 
Print(" 前 方 障碍 物 比较 远 ， 我 可 以 走 的 快 一 些 ") 
slow_walk("forward", 4) 

elif mean_distance > 500: 
print ("我 准备 往 前 走 了 。") 
slow_walk("forward", 1) 

elif mean_distance < 250: 
print ("有 点 近 ， 我 需要 后 退 一 下 ") 
slow_walk("backward", 1) 

else: 
print ("前 方 有 障碍 物 ， 我 准备 右 转 30 度 ") 
slow_walk("rotation", angle=-30) 

WaitForWalkingDone() 


def callback(image) : 
cv image = bridge.imgmsg_to_cv2(image, "16UC1") 


cv image = np.array(cv image) 
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height, width = cv_image.shape 

roi image = cv_image[height / 2 - ROI[1] / 2: height / 2 + ROI[1] / 2, 
width / 2 - ROI[O] / 2: width / 2 + ROI[O] / 2] 

mean_distance = roi_image.mean() 


return mean_distance 


rospy.init, node('roban, avoidance') 
bridge = CvBridge() 

topic = "/camera/depth/image_rect_raw" 
print SetBodyhubTo_walking (2) 


while not rospy.is_shutdown(): 
means_distance = [] 
for _ in range(5): 
means, distance.append( 
callback(rospy.wait, for message(topic, Image))) 


move(sum(means distance) / len(means_distance) ) 
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CHAPTER 7 


几 十 年 来 ， 两 足 机 器 人 的 运动 一 直 是 诸多 研究 者 关注 的 焦点 。 伴 随 着 大 量 的 计算 机 虚拟 仿 
真 和 实物 样机 研究 ， 研 究 者 们 构建 了 各 种 双 足 步 态 规划 与 控制 理论 ， 研 究 内 容 横 跨 最 简单 的 双 
足 平面 机 构 和 各 大 商业 公司 构建 的 复杂 人 形 机 器 人 。 但 不 管 它们 的 具体 结构 和 自由 度 (Degrees 
of Freedom, DoF) 的 数目 如 何 ， 这 些 双 足 机 器 人 系统 都 具有 如 下 共同 特性 。 

(D 在 遭受 强力 扰动 时 ， 整 个 系统 可 能 会 绕 支 撑 脚 的 边沿 旋转 进而 摔 倒 ， 等 同 于 其 系统 内 
部 具有 某 种 被 动 特性 。 

(2) 行走 时 大 致 进行 周期 运动 ， 在 行走 平稳 时 每 一 步 的 行走 状态 类 似 。 

(3) 行走 时 在 左 脚 文 撑 、 双 足 支 撑 、 右 脚 支 撑 之 间 有 规律 地 切换 。 

在 行走 过 程 中 ， 两 种 不 同 的 情况 依次 出 现 : 机 构 同 时 双 脚 支撑 的 静态 稳定 双 支 撑 阶 段 和 单 
腿 支 撑 的 静态 不 稳定 单 支撑 阶段 。 在 单 支 撑 阶 段 ， 机 器 人 只 有 一 只 脚 与 地 面 接触 ， 而 另 一 只 上肢 
从 身体 后 方 转移 到 身体 前 方 ， 即 机 器 人 系统 的 运动 机 构 在 单 次 步行 循环 中 从 开放 式 运 动 链 改变 
为 封闭 式 运动 链 。 因 此 ， 在 对 机 器 人 进行 运动 规划 时 需要 考虑 这 些 情况 。 

双 足 机 器 人 行走 时 脚掌 与 地 面 接触 ， 脚 掌 与 地 面 的 接触 状态 是 不 能 直接 通过 电动 机 等 驱动 
机 构 来 控制 的 ， 但 是 通过 控制 机 器 人 身体 运行 合适 的 运动 轨迹 ， 可 以 间接 控制 脚掌 与 地 面 的 接 
触 状 态 ， 进 而 实现 稳定 的 行走 。 为 此 需要 建立 一 些 指标 来 衡量 脚掌 与 地 面 之 间 的 作用 力 ， 而 零 
力矩 点 〈Zero-Moment Point, ZMP) 就 是 其 中 的 一 个 重要 指标 。 目 前 ， 诸 多 学 者 已 构建 了 基于 
ZMP 的 双 足 步 态 规划 与 控制 方法 。 

本 章 将 简单 介绍 实现 双 足 步行 需要 的 理论 基础 ， 带 各 位 好 奇 的 读者 一 帘 双 足 行走 的 奥秘 。 


7.1 机 器 人 运动 学 


机 器 人 系统 的 运动 主要 有 两 个 方面 的 描述 方式 : 动力 学 和 运动 学 。 动 力学 的 描述 是 更 普 裔 
的 ， 因 为 其 描述 中 引入 了 系统 各 部 件 的 动量 、 互 相 的 作用 力 和 各 自 的 能 量 ， 一 般 用 微分 方程 来 
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描述 系统 的 动力 学 。 运 动 学 的 描述 则 更 简单 ， 因 为 其 只 描述 物体 位 置 与 时 间 相 关 的 变量 ， 在 物 
体 做 义 加 速 运动 时 ， 一 般 用 位 置 、 初 速度 、 末 速度 、 加 速度 、 时 间 5 个 变量 来 描述 物体 的 运动 
学 方程 。 物 体 的 运动 又 分 类 为 平移 、 旋 转 、 振 荡 等 ， 或 者 其 中 数 种 的 组 合 。 本 节 中 我 们 将 用 运 
动 学 的 方式 来 描述 机 器 人 系统 各 部 件 的 平移 和 旋转 运动 。 


7.1.1 ”坐标 变换 


1. 坐标 系 

人 形 机 器 人 的 控制 系统 中 ， 为 了 精确 描述 各 连 杆 部 件 的 位 置 ， 首 先 需 要 建立 一 个 全 局 坐标 
系 ( 也 称 忆 界 坐标 系 )。 为 了 符合 惯例 ， 一 般 建 立 右 手 系 的 坐标 系 ， 即 z 轴 指 向 前 方 ，y 轴 指 向 
右 方 ，z 轴 指 向 上 方 ， 坐 标 系 原点 则 可 以 位 于 机 器 人 双 脚 的 中 间 位 置 ， 如 图 7.1 所 示 。 


7.1 全 局 坐标 系 


手 系 是 建立 坐标 系 时 需要 注意 的 概念 。 让 食指 自然 前 伸 ， 拇 指 侧 伸 与 食指 垂直 ， 中 指 弯 曲 
与 食指 垂直 ， 则 食指 、 拇 指 和 中 指 构成 两 两 垂直 的 3 个 坐标 轴 ， 拇 指 为 z 轴 ， 食 指 为 y 轴 ， 中 
指 为 z 轴 。 左 手 和 右手 分 别 做 该 动作 时 ， 建 立 的 坐标 系 是 不 同 的。 机 器 人 控制 领域 ， 广 泛 使 用 
右手 系 建立 坐标 系 。 不 同 手 系 的 坐标 系 示意 如 图 7.2 所 示 。 

记 建 立 的 全 局 坐标 系 为 2Bw ， 以 此 坐标 系 可 以 描述 机 器 人 各 连 杆 及 其 周边 物体 的 位 置 。 通 
过 检测 周边 环境 ， 机 器 人 即 可 在 全 局 坐标 系 中 完成 物体 抓 取 、 绕 开 障碍 物 等 任务 。 

为 了 控制 机 器 人 各 个 部 件 的 运动 ， 不 仅 需 要 建立 全 局 坐标 系 ， 而 且 当 要 控制 一 个 机 械 辟 前 
伸 到 空间 中 某 个 具体 的 点 时 ， 还 需要 找到 位 于 机 械 辟 末端 的 具体 的 点 作为 控制 对 象 。 机 器 人 运 
动 学 控制 的 普遍 做 法 是 在 各 个 连 杆 部 件 建立 局 部 坐标 系 ， 局 部 坐标 系 随 各 连 杆 的 运动 而 运送 。 
当 各 个 局 部 坐标 系 之 间 的 相对 关系 为 已 知 或 者 可 检测 计算 时 ， 则 机 械 臂 末端 点 在 全 局 坐标 系 下 
的 位 置 也 是 可 得 到 的 ， 进 而 可 以 根据 机 械 臂 末端 点 在 全 局 坐标 系 中 的 实际 位 置 和 想 要 其 到 达 的 
预期 位 置 来 控制 机 械 臂 的 运动 。 图 7.3 所 示 为 以 机 械 臂 为 例 示 意 控制 其 运动 时 需要 建立 的 各 个 坐 
标 系 。 
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图 7.2 左手 系 与 右手 系 

2. 齐 次 变换 矩阵 

物体 在 全 局 坐标 系 中 的 位 置 ， 可 以 用 一 个 位 置 矢 量 来 描述 。 空 间 中 的 物体 还 会 有 不 同 的 朝 
向 ， 描 述 物体 朝向 时 ， 需 要 先 在 物体 上 固定 一 个 坐标 系 ， 再 根据 该 坐标 系 与 全 局 坐标 系 的 朝向 
的 偏差 来 描述 物体 在 全 局 坐标 系 中 的 朝向 。 物 体 的 朝向 也 称 为 姿态 描述 ， 位 置 矢量 p 和 姿态 描 
述 丸 合 称 为 物体 的 位 次 。 图 7.3 所 示 为 局 部 坐标 系 。 


7.3 局 部 坐标 系 


如 图 7.4 所 示 ; 物体 呈 位 于 全 局 坐标 系 Ozys 中 ， 在 物体 上 固定 坐标 系 01,,,,。 三 维 的 位 置 矢 
量 是 一 个 3x1 的 列 向 量 ， 而 坐标 系 Oly 与 坐标 系 Ory, 的 朝向 偏差 则 有 多 种 描述 方式 ， 比 如 
欧 拉 角 、 四 元 数 和 旋转 矩阵 ， 不 同 的 描述 方式 之 间 可 以 相互 转换 。 本 节 只 讲解 旋转 矩阵 的 描述 
方式 。 

考虑 某 个 点 ， 其 在 某 坐 标 系 中 的 位 姿 是 已 知 的 ， 在 另 一 个 坐标 系 中 的 位 姿 是 未 知 的 。 如 果 
两 个 坐标 系 的 相对 位 置 和 相对 姿态 是 已 知 的 ， 则 可 以 通过 坐标 变换 ， 直 接 得 到 点 在 另 一 个 坐标 
系 下 的 位 姿 。 在 机 器 人 运动 学 中 ， 主 要 关心 坐标 变换 中 的 平移 和 旋转 变换 。 
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7.4 ”坐标 系 的 朝向 偏差 


进行 平移 变换 时 ， 只 需要 直接 对 位 置 矢 量 进行 矢量 相 加 即 可 ， 旋 转变 换 则 较为 复杂 。 设 
图 7.4 中 点 P TEARS ES IR Oovz 和 坐标 系 Oly 中 的 位 置 矢 量 分 别 为 


Payz e [ps Py, Pz] 


(7.1) 
Puvw -— [Pu Po. Pu] 


则 有 关系 式 
Poyz zu Roo’ Puvw (7.2) 
Hop, ROSAS O01 与 坐标 系 Owyz 朝向 偏差 的 旋转 矩阵 。 
假如 有 两 个 原点 重合 、 朝 向 不 同 的 坐标 系 ， 可 以 认为 其 是 一 个 坐标 系 依 次 绕 其 zx、y 和 zz 化 
标 轴 旋 转 一 定 角度 得 到 的 。 假 如 只 绕 坐 标 系 Ozy; 的 Oz 轴 旋 转 8 角度 ， 则 两 个 坐标 系 的 旋转 矩 


阵 为 
cos0 —sin@ 0 
Rg= | sin -cos0 .0 (7.3) 
0 0 il 


AGE Oz. Oy 轴 转 动 9 角度 的 旋转 矩阵 分 别 为 


1 0 0 cos? 0 sin 
Rao = | 0 cos@ -sinp Ryo = 0 LH (7.4) 
0 sin cos @ —sinÓÜ 0 cos@ 


由 只 绕 某 个 轴 旋 转 的 基本 旋转 矩阵 相 乘 ， 可 以 得 到 复合 的 旋转 变化 ， 效 果 相 当 于 原 坐 标 系 
绕 其 坐标 轴 进 行 了 多 次 不 同 的 旋转 ， 如 式 (7.5》 所 示 。 


p = R? p, R? = RoR}... Re! (7.5) 


A T WI HREM FEER, ETT BES | A FERRI 2 FEAR PRE ME BA AURRA 
本 身 在 空间 中 的 位 置 和 姿态 ， 也 可 以 表述 不 同 坐 标 系 之 间 的 坐标 变换 。 齐 次 变换 矩阵 是 4x4 的 
HPE, JEU P Br. 
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fixs wri 透视 变换 ”比例 因子 
当 不 进行 透视 变换 和 比例 变换 时 ， 把 透视 变换 的 行 向 量 置 为 0， 把 比例 因子 置 为 1 即 可 。 
使 用 齐 次 变换 矩阵 进行 坐标 变换 示例 如 下 ， 其 中 Poo: 为 由 两 个 坐标 系 原点 构成 的 位 置 矢 
€, Roo: 为 两 个 坐标 系 朝 问 偏差 的 旋转 矩阵 。 


| Pyyz | s | Roo es | Puvw | (7.7) 
1 0 0 0 1 1 
3. 链 乘法 则 


假设 有 N 个 坐标 系 ， 每 两 个 相 邻 坐标 系 Di 和 Dig 之 间 的 齐 次 变换 矩阵 都 是 已 知 的， 为 
iT;y1， 则 依次 进行 如 上 的 齐 次 变换 ， 则 有 


TR TT Ty (7.8) 


Ep, Ty 为 在 最 初始 端 坐标 系 中 表示 的 第 N 个 坐标 系 的 齐 次 变换 矩阵 。 在 机 器 人 运动 学 中 ， 
一 般 最 初始 端 坐标 系 为 全 局 坐标 系 ， 最 末端 坐标 系 为 运动 链 末 端点 坐标 系 ， 如 机 器 人 手掌 、 脚 
掌 、 机 械 辟 的 手 爪 等 。 

上 述 齐 次 变换 矩阵 依次 相 乘 的 计算 方法 被 称 为 坐标 变换 的 链 乘法 则 。 链 乘法 则 使 得 具有 多 
个 关节 的 机 器 人 的 运动 学 计算 简便 化 。 


7.1.2 ”人 形 机 器 人 运动 学 模型 


图 7.5 所 示 为 12 自由 度 双 足 机 器 人 ， 给 机 器 人 各 连 杆 编号 如 图 7.5(a) 所 示 。 可 以 观察 到 该 机 
器 人 髋 关节 的 3 个 关节 转动 轴 相 交 于 一 个 点 ， 躁 关节 的 两 个 转动 轴 相 交 于 一 个 点 ， 这 样 设计 能 
使 机 器 人 运动 学 的 计算 变 得 简便 。 为 了 定义 各 个 连 杆 的 位 姿 ， 需 要 给 每 个 连 杆 设 定局 部 坐标 系 。 
机 器 人 每 条 腿 有 6 个 自由 度 ， 于 是 每 条 腿 设 置 6 个 局 部 坐标 系 。 其 中 3 个 设置 于 髋 关节 转动 轴 
交点 ， 一 个 设置 于 膝 关 节 转 轴 ， 两 个 设置 于 躁 关 节 转 轴 ， 且 局 部 坐标 系 的 各 个 坐标 轴 都 和 全 局 
坐标 系 的 坐标 轴 平 行 。 


7.1.3 ” 正 运动 学 


有 了 机 器 人 模型 之 后 ， 还 需要 根据 模型 求 取 各 个 局 部 坐标 系 之 间 的 齐 次 变换 矩阵 。 如 各 个 
局 部 坐标 系 的 坐标 轴 是 互相 平行 的 ， 则 机 器 人 初始 状态 下 相 邻 坐标 系 之 间 的 旋转 矩阵 为 单位 阵 : 


Rı = R =. -= Rg =E (7.9) 
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(1) BODY 


(8) LLEG_JO 


(10) LLEG_J2 
(4) RLEG_J2 
(3) RLEG_J (9) LLEG J1 
1 
(5) RLEG_J3 (11)LLEG J3 
z 
(6) RLEG_J4 12) LLEG_J4 
] (13) LLEG. J5 y 
(7) RLEG_J5 Xs 
T 
(a) hs 


图 7.5 机 器 人 运动 学 模型 


在 关节 转动 时 ， 每 个 连 杆 上 附着 的 局 部 坐标 系 也 会 跟着 转动 。 定 义 描述 相 邻 局 部 坐标 系 之 
间 关 系 的 关节 轴 矢 量 a; 和 相对 位 置 矢量 b;。 关 节 轴 矢量 是 描述 第 i 个 连 杆 相 对 于 其 母 连 杆 
转动 的 转动 轴 的 单位 矢量 ， 图 7.6 中 as = an = | 0 1 0 | 。 相 对 位 置 矢量 是 描述 第 i 个 
连 杆 的 局 部 坐标 系 原点 在 其 母 连 杆 局 部 坐标 系 中 的 位 置 ， 其 值 的 大 小 和 机 器 人 的 结构 设计 参数 
有 关 。 


图 7.6 关节 轴 矢 量 和 相对 位 置 矢量 


不 同 于 传统 的 DH 法 描述 的 机 器 人 运动 学 模型 ， 基 于 关节 轴 矢 量 和 相对 位 置 矢量 的 描述 方 
法 非常 简便 且 强大 。 本 节 直 接 介绍 基于 该 方法 的 齐 次 变换 矩阵 的 计算 方法 ， 并 进行 机 器 人 的 正 
运动 学 求解 。 

考虑 原点 附着 于 第 i 个 关节 转动 轴 上 、 随 第 i 个 连 杆 运动 的 局 部 坐标 系 也;， 当 关节 i 的 转 
AREA OW, X; 在 母 连 杆 局 部 坐标 系 Di 下 的 姿态 矩阵 为 单位 阵 百 。 当 关节 转动 角度 为 qj 
IY, Dy 相对 于 母 连 杆 的 齐 次 变换 矩阵 可 以 直接 求 出 : 


EEC 
Tm (7.10) 
be 05 pe 4 


其 中 ， 在 关节 轴 矢 量 上 加 帽子 符号 的 a; Bean = ES HH EO RT EE, REA 


^ 


Wr 0 —u Wy 
ð= | wy = Wz 0 —wr (7.11) 
We =Wy We 0 


把 斜 对 称 矩 阵 放 在 自然 对 数 e 的 指数 位 置 ， 表 示 竹 阵 指数 。 和 矩阵 指数 可 以 用 罗 德 里 格 斯 旋 
转 公式 来 简化 计算 ， 其 表示 把 角速度 矢量 直接 转换 为 旋转 矩阵 的 操作 ， 具 体 为 


eĉ? = E + o sin0 + à? (1— cos0) (7.12) 


得 到 27; FART BEER IFT eR EZ Je, TRAE HT Jo UAE ERR. D 相对 于 全 局 坐标 系 
中 的 位 置 p; 和 姿态 Ri 的 已 知 ， 则 其 齐 次 变换 矩阵 为 


=| ER ^ (7.13) 
000 1 


根据 链 乘法 则 ， 马 相对 于 全 局 坐标 系 的 齐 次 变换 矩阵 可 以 直接 得 到 
T; =T'T; (7.14) 
2j 相对 于 全 局 坐标 系 的 位 置 和 姿态 可 得 : 


pj; = Pi + Rib; 


Rj = Rieti: Ji: 


如 果 机 器 人 在 某 个 姿态 下 ， 有 一 个 连 杆 相对 于 全 局 坐标 系 的 位 置 是 已 知 的 ， 则 通过 以 上 公 
式 可 以 依次 计算 出 其 他 机 器 人 其 他 连 杆 在 全 局 坐标 系 下 的 位 置 ， 这 就 是 机 器 人 的 正 运动 学 计算 。 
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在 双 足 行走 过 程 中 ， 一 般 假定 支撑 脚 脚掌 在 地 面 的 位 置 是 已 知 的 ， 以 此 来 进行 全 身 的 正 运 动 学 
求解 。 

7.44 ” 逆 运 动 学 

正 运动 学 是 已 知 机 器 人 运动 学 模型 的 情况 下 ， 根 据 机 器 人 各 关节 角度 求 各 连 杆 的 位 姿 。 逆 
运动 学 则 相反 ， 是 确定 想 要 某 连 杆 达 到 的 预期 位 姿 ， 根 据 预期 位 姿 求解 该 状态 下 机 器 人 各 关节 
的 角度 。 一 般 是 给 出 机 器 人 脚掌 或 手掌 连 杆 的 预期 位 姿 ， 然 后 求解 各 个 关节 的 角度 。 

1. 逆 运 动 学 的 解析 解法 

在 机 器 人 髋 关节 三 轴 相 交 、 踪 美 节 两 轴 相 交 的 情况 下 ， 可 以 比较 简单 地 用 解析 法 求解 腿 上 
各 个 关节 的 转角 。 如 图 7.7 所 示 , 定义 从 锯 干 坐标 系 的 原点 到 髋 关节 的 距离 为 D, 大 腿 长 为 A 小 
腿 长 为 Bo 给 定 抠 干 和 右 脚 的 位 姿 分 别 为 (ps, Ri) 和 (pz, Rz). 


7.7 解析 法 求解 逆 运 动 学 


此 时 依次 求 得 休 关 节 位 置 为 


0 
P2=P,+ Hi | D (7.16) 
0 


BRIS AGER S FRAR TIL EL A ERA 
£X 
r=R} (pa-p) = | ra ry ra] ain 


躁 关节 与 通关 节 之 间 的 距离 C 为 


OV (7.18) 
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根据 三 角形 余弦 定理 ， 有 
C? = A? + B? — 2AB cos (n — qs) (7.19) 


从 而 求 得 膝 关 节 角 qs 为 


2 2. 2 
A*+B =) f (7.20) 


q5 = — arccos (“a + 
基于 求 得 的 gs 和 rer ry ro TURE = Ae RH IEA qe. qr 分 
别 为 : 了 
(7.21) 
qe = — atan 2 (ro, sign (rz) /To 4: r2] — a 


其 中 


& = — arcsin (mcm) 


(7.22) 
最 后 再 求 得 通关 节 角 q2* q3~ a 分 别 为 : 


q2 = atan 2 (—Rj2, R22) 
q3 = atan 2 (R32, — R1282 + R22¢2) (7.23) 
q4 = atan 2 (—R31, R33) 


KB, Ry 为 如 下 矩阵 的 元 素 ; 为 了 简写 ，c2 = cos go， so = sin qz. 


R, (q2) Rz (q3) Ry (q4) = Ri Rz Rs (qz) Ry (qs + qc) (7.24) 
C2C4 — 828384 一 52C3 C254 十 8253C4 Ru Ri Rig 
82C4 十 c25354 C2C3 58284 — C233C4 | = | Roi Roo Rog (7.25) 
—C384 83 C3C4 Hai R32 R33 


DA EBD yi a) PTZ: ASHER, VARÉDU ODD SR AM — ATA. BR 
两 轴 相 交 ， 这 会 大 大 减 小 解析 法 计算 的 复杂 程度 。 实 际 的 机 器 人 运动 控制 过 程 中 ， 一 般 使 用 数 
值 解 法 进行 逆 运 动 学 求解 。 

2. 逆 运 动 学 的 数值 解法 

解析 解法 求解 逆 运 动 学 原理 简单 上 且 计算 量 小 ， 但 其 应 用 的 局 限 性 较 大 。 比 如 ， 对 于 一 些 特 
殊 构 型 的 机 器 人 ， 逆 运动 学 可 能 得 不 到 解析 解 。 数 值 解法 则 适用 范围 更 广 ， 虽 然 迭 代 计 算 求解 
需要 更 大 的 计算 量 ， 但 现 有 的 计算 芯片 可 以 轻易 满足 逆 运 动 学 数值 解法 的 计算 需求 。 

首先 考虑 一 六 自由 度 的 运动 机 构 ， 因 为 不 管 是 机 械 臂 还 是 机 械 腿 ， 在 逆 解 时 都 可 以 被 视 为 
同样 的 对 象 。 简 单 的 逆 解 情况 ， 可 以 认为 逆 解 时 机 构 的 一 端 固定 另 一 端 运 动 。 固 定 的 一 端 称 为 
基 座 , 运动 的 一 端 称 为 末端 。 末端 在 三 维 空间 中 运动 时 ， 其 位 姿 也 在 变化 , 我 们 称 末 端 在 “ 箔 卡 
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儿 空 间 ” 中 运动 。 随 着 末端 在 空间 中 和 运动， 机 构 的 各 个 关节 的 转动 角度 也 在 变化 。 末 端的 每 个 
空间 姿态 ， 对 应 有 相应 的 各 个 关节 的 转动 角度 ， 我 们 称 由 各 个 关节 转动 角度 构成 的 矢量 在 “ 关 
节 空 间 ” 中 运动 。 于 是 正解 是 把 关节 空间 的 关节 转角 矢量 转化 为 笛 卡 儿 空间 的 末端 位 姿 ， 逆 解 
则 是 反 过 来 从 末端 位 姿 求解 关节 转角 矢量 ， 数 学 式 的 表达 则 为 


x= f(q) (7.26) 
其 中 ，z 为 末端 的 空间 位 置 和 朝向 角 ; q 为 关节 转角 矢量 。 


T q2 


e 
2 


r= g= (7.27) 


遗憾 的 是 ， 没 有 办 法 具体 地 写 出 式 〈7.26) 中 函数 f() 的 形式 ， 所 以 不 能 直接 使 用 函数 计算 
正 逆 解 。 不 过 ， 可 以 通过 另 一 个 可 求 得 的 矩阵 ， 即 雅 可 比 矩 阵 ， 来 进行 逆 解 的 求解 。 雅 可 比 拢 
阵 的 含义 是 把 运动 链 末 端点 在 笛 卡 儿 空间 的 运动 速度 ， 映 射 到 关节 空间 的 关节 转角 速度 ， 其 数 
学 形式 为 


dz =J -dq (7.28) 


根据 式 (7.28) 可 以 认为 未 端点 在 笛 卡 儿 空间 中 有 一 个 位 移 Az 时 ， 对 应 的 关节 转角 矢量 有 
一 个 变化 的 差 值 Ag。 当 位 移 Ar i), HS Ad 的 关系 就 越 符 合式 〈7.28)。 图 7.8 展 示 了 在 位 
移 大 小 变化 时 ， 由 Ag 产生 的 实际 未 端 轨迹 与 理想 位 移 轨迹 的 偏差 。 可 以 看 出 ， 位 移 越 小 时 二 
者 轨迹 偏差 越 小 。 í 

利用 该 关系 进行 逆 解 求解 的 过 程 为 , 首先 计算 运动 机 构 的 当前 位 姿 与 预期 位 姿 的 差 值 dz 和 
机 构 当 前 位 次 下 的 雅 可 比 矩阵 J， 然 后 利用 式 〈7.28) 计算 对 应 的 关节 转角 矢量 增 量 dg， 接 着 
把 增 量 dq 加 到 当前 关节 转角 矢量 q 上 重新 计算 新 的 运动 机 构 位 姿 。 显 然 ， 新 的 位 姿 会 比 原 先 
的 位 姿 更 接近 预期 位 姿 。 以 上 流程 经 过 多 次 迭代 ， 使 最 后 求 得 的 位 姿 偏 差 接近 符合 预先 设 定 的 
精度 要 求 时 ， 即 认为 求解 成 功 。 


其 中 ， 根 据 式 〈7.28) 计算 关节 转角 矢量 增 量 dg 时 ， 自 然而 然 的 做 法 是 求 取 雅 可 比 矩 阵 J 
的 逆 ， 从 而 得 到 dq。 但 在 许多 情况 下 ， 因 为 运动 机 构 构 型 或 者 机 构 位 姿 的 差异 ， 雅 可 比 和 矩阵 是 
不 可 逆 的 。 为 了 处 理 该 问题 ， 根 据 不 同 的 处 理 手段 又 引申 出 了 雅 可 比 转 置 法 、 伪 逆 法 、 奇 异 值 
分 解法 等 逆 解 的 数值 解法 ， 本 节 不 再 详细 介绍 。 
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实际 运动 轨迹 线性 近似 轨迹 


O,+40, 
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接 下 来 以 MATLAB 软件 的 机 器 人 工具 箱 中 的 ikineQ 逆 解 函数 为 例 ， 说 明 上 述 计算 方法 的 

编程 实现 流程 ， 如 算法 1 所 示 。 
Algorithm 1 InverseKinematics 

I: To 初始 化 机 器 人 模型 ; 

2: 9o 设 定 目 标 位 姿 荆 、 精 度 要 求 tolerance 等 ; 

3: % 初始 化 位 姿 误 差 ; 

4: while dz > tolerance do 

5: So VESURERI EHRE J; 

: “证 使 用 雅 可 比 伪 道 法 then 


6 
7 % 使 用 雅 可 比 伪 逆 法 计算 关节 转角 矢量 增 量 dq; 
8: else 

9: % 使 用 雅 可 比 转 置 法 计算 关节 转角 矢量 增 量 dq; 
10: end if 

11: %o BE gq; 

2: — 9e IEEE 

13: ”更 新 位 姿 误 差 dz; 


14: end while 
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7.2 ZMP 的 含义 


零 力矩 点 (ZMP) 是 一 个 适用 于 各 种 运动 状况 下 的 概念 ?， 也 是 对 人 形 机 器 人 进行 运动 控制 
时 需要 用 到 的 一 个 重要 物理 量 。 本 节 首 先 介绍 ZMP 的 定义 、 计 算 方 式 和 测量 方法 ， 然 后 讨论 
ZMP 指标 与 人 形 机 器 人 动力 学 的 关系 ， 讲 解 ZMP 在 机 器 人 运动 时 是 如 何 发 挥 作用 的 。 


7.2.1 ZMP 与 地 面 反 力 


不 同 于 工业 机 器 人 的 基 座 固定 于 地 面 ， 人 形 机 器 人 脚掌 与 地 面 为 不 稳定 接触 ， 因 此 人 形 机 
器 人 会 轻易 地 摔 倒 。 为 了 控制 机 器 人 稳定 行走 ， 需 要 建立 一 些 指标 来 判断 机 器 人 是 否 稳定 。 如 
果 当 这 些 指标 在 一 定 范 围 内 时 ， 机 器 人 一 定 不 会 摔 倒 ， 则 该 指标 就 能 为 机 器 人 的 运动 控制 提供 
极 大 的 帮助 。 这 种 情况 下 ， 人 们 常常 使 用 ZMP。 

如 图 7.9 所 示 ， 机 器 人 足 底 所 受 作用 力 分 布 不 均匀 ， 肢 尖 方 向 受到 的 作用 力 较 大 ， 脚 跟 方向 
较 小 。 但 整体 的 负载 可 以 等 效 于 一 个 作用 于 足 底 某 个 位 置 的 合力 R, HEERE REH ARE 
HEJH. MER ZMP. 


ZMP 


图 7.9 IEA (ZMP) 的 定义 


上 述 是 三 维 的 情况 ， 即 只 考虑 了 脚掌 前 后 方向 的 受 力 和 ZMP 位 置 。 实 际 上 ， 脚 掌 的 左 、 右 
方向 也 会 受 力 ， 左 、 右 方向 的 ZMP 位 置 也 会 随 受 力 情 况 的 变化 而 变化 。 接 下 来 考虑 三 维 环境 下 
单 足 支撑 的 情况 ， 如 图 7.10(a) 所 示 。 为 简化 分 析 ， 可 以 忽略 躁 关节 上 部 的 结构 。 把 中 关节 以 上 
的 结构 对 躁 关 节 的 影响 等 效 为 作用 力 FP A 和 作用 力矩 Ma， 如 图 7.10(b) 所 示 。 

通常 ,地面 反 力 由 力 R 的 3 个 方向 的 分 量 Re Ry R: MIHE M 的 3 个 方 则 Mz、My、 
M: 构成。 摩擦 力作 用 于 脚 与 地 面 的 接触 点 , 当 脚 在 地 面 上 处 于 静止 状态 时 , 作用 于 水 平面 上 的 
Jj RAJE M 的 那些 分 量 会 被 地 面 摩 擦 力 平衡 抵消 。 因此 , 水 平方 向 的 地 面 反 作用 力 (Rs, Ry) 


O 高 达 SEED 第 一 季 中 ， 主 角 操控 高 达 时 即 出 现 了 “校准 零 力矩 点 ”的 台词 。 


表示 平衡 Fa 的 水 平分 量 的 摩擦 力 。 而 垂直 方向 的 反 力 矩 M, 表示 地 面 摩擦 反 力 矩 , 它 平衡 力 
AE MA 的 垂直 分 量 和 力 FA 引起 的 力矩 ， 如 图 7.10(c) 所 示 。 竖 直方 向 的 地 面 反作用 力 R, 则 平 
衡 Fa 的 竖 直 方向 分 量 ， 当 机 器 人 没有 竖 直 方向 的 加 速 运动 时 ，R, 的 值 等 于 机 器 人 的 重力 大 
小 。 此 时 还 需要 考虑 的 是 M A 的 水 平方 向 的 力矩 分 量 M Ar 和 M ay 的 平衡 问题 。 

考虑 如 图 7.10(d) 所 示 的 Oyz 平面 ， 通 过 调整 R, 的 作用 点 可 以 等 效 地 生成 水 平方 向 的 
力矩 分 量 来 平衡 Mas。 生 成 的 平衡 力矩 分 量 的 大 小 取决 于 点 P 与 躁 关节 在 Oyz 平面 的 相对 距 
离 y。 此 时 可 以 发 现 ， 当 地 面 反作用 力 始 终 在 脚掌 所 覆盖 的 区 域内 时 , 中 关 节 受 到 的 地 面 反 力矩 
可 以 等 效 为 通过 改变 R, 的 作用 位 置 生成 的 水 平 扭矩 分 量 。R 的 作用 位 置 改 变 之 后 ， 则 可 以 认 
为 ， 地 面 反 力矩 的 水 平分 量 Ms SIM, 不 存在 了 。 


Q 


图 7.10 三 维 地 面 反 力 分 析 


需要 注意 的 是 ， 对 于 实际 的 机 器 人 来 说 ， 作 用 点 PP 的 移动 距离 是 受 限 制 的。 其 受制 于 机 器 
人 脚掌 的 实际 大 小 ， 不 能 移出 脚掌 的 支撑 面 之 外 。 直 观 的 理解 是 ， 如 果 作 用 点 已 移出 了 脚掌 支 
撑 面 之 外 ， 地 面 反作用 力 就 不 能 通过 脚掌 传递 至 中 关节 ， 进 而 平衡 系统 受 力 了 。 为 此 需要 引入 
另 一 个 重要 概念 一 一 支撑 多 边 形 ， 其 定义 为 脚掌 与 地 面 接 触 点 的 集合 的 最 小 凸 集 。 山 集 的 定义 
如 图 7.11 和 式 〈7.29) 所 示 ， 双 足 机 器 人 的 支撑 多 边 形 如 图 7.12 所 示 。 
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j=l 


N N 
Sco = 人 这 see = 1,p; € S(j =1,2,--- | (7.29) 
ja 


图 7.11 nÆ 


图 7.12 双 足 机 器 人 的 支撑 多 边 形 


在 本 节 我 们 先 给 出 一 个 重要 结论 :“ 机 器 人 保持 稳定 时 ，ZMP 位 于 支撑 多 边 形 内 部 ”，7.2.2 
节 来 分 析 该 结论 是 如 何 得 出 的 。 


7.2.2 ZMP 分 析 


1. 二 维 平面 分 析 

本 节 我 们 采用 更 为 严谨 的 公式 ， 先 来 分 析 二 维 平面 下 ZMP 的 位 置 与 足 底 受 力 情 况 的 关系 ， 
之 后 再 把 该 方法 拓展 到 三 维 空间 。 如 图 7.13 所 示 ， 由 于 机 器 人 脚掌 受到 地 面 的 摩擦 力 , 地 面 作用 
力 有 竖 直 和 水 平方 向 的 分 量 ， 在 图 7.13(a) 和 图 7.13(b) 中 分 别 用 pE) 和 o(£) 来 表示 每 单位 距离 
的 地 面 反 作用 力 的 竖 直 方向 和 水 平方 向 分 量 。 


Z ol(é) 
p(é) Ls 
Ti [ T) 
(b) 


743 地 面 作用 力 的 分 布 


(a) 


aw mEsHEM M ae 


上 述 的 力 分 量 同 时 作用 于 机 器 人 足 底 。 接 着 我 们 用 集中 作用 在 脚底 上 某 一 个 点 的 等 效力 和 
力矩 ， 来 替换 图 7.13 中 的 分 布 力 ， 如 图 7.14 所 示 。 


744 ”等 效力 和 等 效力 矩 


此 时 等 效力 和 等 效力 矩 包括 水 平分 量 记 、 竖 直 分 量 fe 和 绕 作 用 点 ps HIE (ps) 用 如 下 
公式 计算 : T2 
Le / o(€)dé 


1 


gu n * p(£)d£ (7.30) 


rij) - | Ww S gore 


根据 式 (7.30)， 显 然 存 在 菜 个 特殊 的 作用 点 内 ， 使 得 等 效力 矩 为 零 ， 即 有 r (ps) = 0。 此 
时 pz 的 值 为 a 
/ éo(€)aé 
CNET oie ae (7.31) 
[ p(é)dé 
由 于 脚掌 与 地 面 之 间 为 单 边 接触 , 即 脚掌 不 能 往 下 深入 地 面 , 但 脚掌 可 以 往 上 脱离 地 面 , 所 
以 地 面 作用 力 的 分 量 一 定 为 正 ， 因 此 有 


p(&) 2 0 (7.32) 


代入 式 (7.31), AE 
T1 € Pr S T2 (7.33) 


即 当 全 部 压力 都 位 于 脚尖 的 时 候 ， 除 了 ple) 不 等 于 零 ， 其 他 p(é&) MAS, Ap, = 22. HER 
压力 都 位 于 脚跟 的 时 候 ， 除 了 p(z1) 不 等 于 零 ， 其 他 p(£) BAS, p, = 21。 当 脚掌 压力 分 散 
分 布 于 脚掌 面 时 ，pz 位 于 xi 与 za 之 间 。 这 表明 ， 在 二 维 情况 下 当 通 过 移动 等 效力 的 作用 点 能 
实现 平衡 时 ， 作 用 点 会 在 脚掌 的 范围 内 。 
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2. 三 维 空间 分 析 


机 器 人 脚掌 在 三 维 空间 中 运动 时 ， 脚 掌 姿态 和 受 力 情况 会 比 二 维 情况 更 复杂 ， 但 三 维 空间 
中 的 ZMP 原理 仍然 类 似 。 考 虑 机 器 人 脚掌 位 于 三 维 空间 中 的 水 平地 面 上 ,其 竖 直 方向 和 水 平方 
向 作用 力 分 量 分 别 如 图 7.15(a) 和 (b) 所 示 。 


图 7.15 三 维 空间 中 的 地 面 作用 力 分 量 


设 地 面 上 某 点 > = |e n 0 |] ， 其 中 和 六 分别 为 该 点 在 o 轴 和 办 的 坐标 值 。 再 设 
pe 作为 该 点 处 地 面 反作用 力 的 坚 直 分 量 的 大 小 ， 如 图 7.15(a) 所 示 。 地 面 作用 力 的 竖 直 方向 分 
量 的 总 和 为 

Ter [ p(£, )dS (7.34) 
S 


类 似 式 730) ， 可 以 计算 三 维 情况 下 ， 地 面 作用 力 绕 某 点 p = | pe p 0] 的 力 抵 
Tn(p): 


Tn(D) = ax Tny Tas]. 


Taz = | (n — Py) plé, mdS 


t 


(7.35) 
Tny = — 人 (£ ET Pz) plé, mds 


Tag =O 


Bh, f. 表示 对 地 面 作用 力 进行 二 重 积 分 。 地 面 反 力 的 竖 直 分 量 不 会 造成 绕 z 轴 转 动 的 力 
Ki, MU, = 0。 同 样 如 同 三 维 的 情况 ， 为 使 等 效力 矩 为 零 ， 即 


(7.36) 


于 是 有 
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£p(£, njas 
] «eas 
f np(&, nds 


Py = 
人 p(é,mds 


此 时 点 p 等 价 于 地 面 反 力 的 集中 作用 点 ， 且 此 时 地 面 反 力 绕 点 p 的 力矩 为 零 ， 机 器 人 不 会 
有 绕 点 p 转动 的 趋势 ， 点 p 即 为 此 时 的 ZMP。 

需要 注意 的 是 ， 与 三 维 情况 不 同 ， 三 维 情况 下 地 面 作用 力 的 水 平分 量 ， 会 对 脚掌 产生 等 效 
力 窍 。 考 虑 如 图 7.15(b) 的 水 平方 向 作用 力 分 量 ， 设 olé, n) 和 oy(E,n) 分 别 为 单位 面积 上 的 水 
平分 量 在 z Al y 方向 上 的 分 量 ， 则 两 个 方向 的 水 平 力 分 别 为 


Pz = 


(7.37) 


Ad [ oa(é,n)aS 


(7.38) 
fy= | ose jas 
于 是 水 平 作用 力 分 量 产生 的 绕 地 面 上 点 p= | p。 py 0| MUROS 
Tt(D) = [Tex Tty Tiel 
Tha = 0 
Ty =0 (7.39) 


rte = [ I pain) — t caps term) ds 


可 见 ， 地 面 作 用 力 的 水 平方 向 作用 力 分 量 ， 产 生 了 沿 紧 直 方向 的 力矩 。 一 般 情 况 下 该 竖 直 
方向 的 力矩 会 被 静摩擦 造成 的 地 面 作用 力矩 平衡 ， 如 图 7.10(c) 所 示 ， 当 该 力矩 较 大 静摩擦 不 足 
以 平衡 时 ， 则 会 造成 机 嚣 人 绕 z 轴 方 向 的 转动 。 一 般 情 况 下 机 器 人 以 较 慢 速度 行走 时 ， 忽 略 Tu 
对 机 器 人 的 影响 。 

综 上 可 以 总 结 ， 分 布 在 脚掌 上 的 地 面 反 作用 力 可 以 用 下 面 的 力 


fS s] (7.40) 


和 绕 ZMP (p 点 ) WHE 
Tp =Tn(p) + Tilp) 
* (7.41) 

= | Oo | 


252 如 仿 人 机 器 人 建 模 与 控制 


来 等 效 普 换 。 在 三 维 情况 下 ，ZMP 定义 为 使 地 面 作用 力 的 力矩 水 平分 量 为 零 的 作用 点 。 


7.2.3 ZMP 的 测量 


在 一 些 运动 控制 算法 中 ， 机 器 人 行走 时 需要 对 足 底 ZMP 位 置 进行 实时 测量 。 测 量 ZMP 时 
和 需要 考虑 不 同 的 情况 ， 从 支撑 腿 的 数目 分 类 可 分 为 单 脚 支撑 的 测量 场景 和 多 腿 支 撑 的 测量 场景 
(除了 双 足 机 器 人 ， 四 足 、 六 足 等 机 器 人 也 可 以 进行 ZMP 的 测量 ); 从 测量 方案 上 分 类 ， 可 分 
为 基于 单个 多 维 力 /力矩 传感器 (FT 传感器 ) 的 测量 和 基于 多 个 力 传 感 记录 单元 (Force Sensing 
Register, FSR) 的 测量 。 

首先 描述 ZMP 测量 的 原理 。 考 虑 两 个 一 上 一 下 互相 接触 的 刚体 ， 其 中 之 一 与 地 面 接触 ， 如 
图 7.16 所 示 。 地 面 接触 力 通 过 下 方 物体 传导 至 上 方 物体 ， 此 时 可 以 在 两 个 物体 之 间 布 置 力 传 感 
器 ， 测 量 两 个 物体 之 间 传 导 的 力 和 力矩 的 大 小 。 利 用 测 得 的 力 和 力矩 的 大 小 ， 即 可 计算 当前 时 
刻 的 ZMP 位 置 。 


图 7.16 互相 接触 进行 力 传导 的 刚体 模型 


设 在 脚掌 坐标 系 中 的 点 pj = 1,2,… ,入 ) 处 ， 力 f; MAR m; 的 值 已 测 得 ， 那 么 绕 任意 
T 
点 了 = | p。 py pz | 的 合力 逢 为 


N 
r(p = (pj —p) x f5 +75 (7.42) 
j=l 
ADM x AA y HOT A, BTR po 和 py 的 位 置 : 
N 
Y {ny (piz — Pz) fje + Piaf.) 


EN 

AT N 
2 fiz 
j=l 


Px 
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N 
Yo (ns — (pja — pz) fiy Pay fie} 
noce o e v o oo (7.43) 


N 
bg 
q=1 


不 同 的 测量 方案 都 是 使 用 该 公式 来 进行 ZMP 的 计算 。 下 面 分 别 介绍 基于 两 种 测量 方案 的 单 
腿 支 撑 场 景 下 的 ZMP 测量 和 单 腿 ZMP 已 知 时 的 多 腿 支 撑 ZMP 计算 。 
1. 使 用 六 维 力 传 感 器 测量 单 腿 ZMP 
对 单 腿 进行 ZMP 测 量 时 ， 使 用 一 个 六 维 力 传感器 即 可 测量 得 到 ZMP。 图 7.17 是 六 维 力 传 感 
器 的 工作 示意 图 。 商 品 化 的 六 维 力 传感器 能 做 到 结构 紧凑 坚固 ， 能 承受 较 大 的 冲击 。 
六 维 力 / 力矩 传感器 
[mm a, 


he NET 


图 7.17 六 维 力 传感器 


传感器 被 安装 在 机 器 人 足 底 的 安装 结构 如 图 7.18 所 示 。 施 加 于 足 底 的 地 面 作 用 力 通过 冲击 
吸收 器 和 缓冲 器 传递 到 传感器 上 ， 通 过 该 传感器 作用 力 又 传递 到 机 器 人 中 关节， 进而 影响 机 器 
ASEF RSS Zl). 


图 7.18 六 维 力 传感器 足 部 安装 结构 


T 


六 维 力 传感器 测量 值 包括 三 维 力 了 = | f fy n] msemir-[u n n] 
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六 维 力 传感器 得 到 数据 之 后 ， 根 据 式 (7.43) 计算 ZMP 位 置 。 当 传感器 的 测量 中 心 正 好 位 于 
脚掌 坐标 系 的 原点 正 上 方 时 ， 计 算 ZMP 位 置 位 置 最 容易 : 
Pz = (—Ty = fxd) [fe 
Py = (Te E fyd) / f« 


(7.44) 


其 中 ，d 为 图 7.17 中 六 维 力 传感器 到 足 底 的 距离 。 


2. 使 用 多 个 FSR 单元 测量 单 腿 ZMP 


高 精度 的 六 维 力 传感器 体积 较 大 ， 且 价格 昂贵 。 为 了 使 足 部 结构 重量 轻巧 且 节 省 成 本 ， 可 
以 使 用 多 个 FSR 单元 来 测量 ZMP. FSR 单元 的 主要 测量 元 件 是 压 感 电 阻 ， 压 感 电阻 会 随 着 受到 
的 正 压 力 而 改变 阻 值 ， 因 此 相当 于 一 个 一 维 力 传感器 ， 可 以 测量 该 点 的 地 面 作用 力 的 竖 直 分 量 。 
由 于 单个 FSR 单元 不 能 测量 脚掌 受到 的 扭矩 ， 所 以 需要 配置 多 个 FSR 单元 来 间接 测量 总 的 地 面 
作用 力矩 。 其 工作 原理 示意 图 和 安装 结构 示意 图 ， 分 别 如 图 7.19 和 图 7.20 所 示 。 


图 7.20 FSR 单元 安装 结构 


在 传递 地 面 作用 力 时 ， 每 个 FSR 单元 都 被 视 为 点 接触 ， 只 传递 力 而 不 转 递 力矩 ， 所 以 仅 能 
测量 到 z 方向 的 地 面 作用 力 的 分 量 。 但 由 于 配置 了 多 个 FSR 单元 ， 且 没 个 FSR 单元 在 脚掌 坐标 
系 中 的 位 置 已 知 ， 所 以 可 以 间接 计算 得 到 整个 脚掌 受到 的 合力 符 。 式 〈7.43) 中 ， 没 个 测量 点 仅 
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有 “方向 的 地 面 作用 力 分 量 不 为 零 ， 其 他 都 为 零 。 可 求 得 ZMP X: 


Pc = 


A (7.45) 


3. 双 腿 支撑 的 ZMP i1 
测量 得 到 单 脚 的 ZMP 位 置 之 后 , 即 可 计算 双 脚 支撑 情况 下 的 ZMP。 双 足 支撑 情况 下 的 ZMP 
计算 原理 如 图 7.21 所 示 。 


721 双 足 支撑 情况 下 的 ZMP 


同样 根据 式 〈7.43) 可 求 得 : 


= Prof Rz SE Dial bz 


Me 0.46 
i fre + frz 
其 中 ， 
dys | ra vend 
fi | tre fiv, for | (141) 
Phu | PRr PRy PRz (i 


分 别 为 左 腿 右 腿 的 支撑 力 和 左 腿 右 腿 的 ZMP 位 置 。 支 撑 力 沿 z 轴 和 y 轴 的 分 量 在 计算 ZMP 时 
不 需要 用 到 ， 可 以 不 用 测 出 。 
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7.2.4 ZMP 与 机 器 人 运动 


借 由 介绍 ZMP， 我 们 开始 了 解 机 器 人 运动 时 的 受 力 情况 。 通 过 脚掌 传递 给 机 器 人 的 地 面 作 
用 力 ， 最 终 会 对 机 器 人 躯干 的 运动 状态 产生 影响 ， 而 这 些 影响 遵循 了 哪些 物理 定律 ”需要 怎样 
来 量化 计算 ? 根据 牛顿 定律 可 知 ,受到 作用 力 后 物理 上 产生 加 速度 , 进而 导致 物体 速度 、 位 置 变 
化 。 反 过 来 ， 如 果 知道 位 置 、 速 度 、 加 速度 等 状态 信息 ， 也 可 以 推算 出 物体 受到 的 地 面 作用 力 ， 
进而 计算 出 各 个 时 刻 的 ZMP 位 置 。 为 了 解 上 述 因素 背后 的 作用 原理 ， 本 节 来 探讨 ZMP 与 机 器 
人 运动 的 关系 。 阅 读本 节 时 读者 需要 先 大 致 了 解 多 刚体 动力 学 中 的 机 器 人 动量 、 角 动量 、 质 心 
等 概念 。 

1, ZMP 与 动量 、 角 动量 和 质心 

假设 一 个 人 形 机 器 人 在 水 平地 面 运动 ， 有 一 个 测量 机 器 可 以 用 近乎 上 帝 视角 一 般 的 测量 能 
力 ,准确 地 测 出 某 机 器 人 的 运动 状态 (在 仿真 环境 中 可 以 轻松 实现 )。 考 虑 在 某 一 个 瞬间 时 刻 的 
情况 ， 认 为 此 时 机 器 人 各 个 部 件 之 间 没 有 相对 运动 ， 机 器 人 整体 视 为 一 个 刚体 。 此 时 地 面 作用 
力 绕 原 点 的 力矩 为 

r=pxf (7.48) 


HH, p= [ps,py,Pz] 为 ZMP 位 置 ， 为 等 效 的 地 面 作用 力 的 竖 直 分 量 。 根 据 牛 顿 运 动 定律 ， 
物体 动量 的 变化 率 等 于 物体 受到 的 合 外 力 ， 则 有 


EEMI (1.49) 
L=cxMg+r 


PIE (7.48) AIL (7.49), WEA 了 和 力矩 +7， 可 得 
Ly + Mgy + Pypz 一 (È. + Mg) py zu 
s a i (7.50) 
Ly — Mgr — Pap: + (È. + Mg) Pa = 0 


其 中 ， 


(7.51) 


分 别 为 物体 的 动量 、 角 动量 、 质 心 位 置 和 重力 加 速度 。 从 式 〈7.50) 解 得 ZMP 为 


E- Mgz + p. P, 一 E, 
Mg+P. 


T 
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_ Mgy t pP, + Ls 
Mg4- P, 

从 式 (7.52) 可 看 出 ，ZMP tre SHLAA DAL a) UR s CHR RAK. SPL AGER 
止 不 动 时 ， 其 动量 / 角 动 量变 化 率 为 零 ， 此 时 ZMP 位 置 等 于 机 器 人 质心 位 置 。 

2. 多 刚体 系统 中 的 ZMP 

在 多 刚体 系统 中 ， 机 器 人 的 动量 和 角 动 量 不 能 直接 测 得 ， 但 是 可 以 先 测 得 机 器 人 各 个 连 杆 
部 件 的 速度 和 角速度 ， 进 而 计算 总 的 动量 和 角 动 量 。 现 有 的 各 种 状态 估计 方法 可 以 较为 准确 地 
测量 实体 机 器 人 的 运动 状态 ， 本 书 不 做 介绍 。 

假定 有 一 个 多 连 杆 组 成 的 机 器 人 ,其 各 个 部 件 的 运动 状态 都 已 测 得 ， 如 图 7.22 所 示 ， 则 机 器 
人 整体 绕 坐 标 远 点 的 角 动 量 为 


Dy (7.52) 


N 
=y akh] (7.53) 
TT 


"T T 


其 中 , ci = | a; y a | 为 各 个 连 杆 的 质心 位 置 ，P; = | wi; Mj. ME | 为 各 个 连 杆 
的 动量 变化 率 。 将 式 〈7.53) 代入 式 〈7.52)， 即 可 得 到 多 刚体 系统 的 ZMP 计算 公式 ; 


N 
M UG + 9) zi — (zi — pz) ži} 
p, = = z CHRIDOEDZCUE Pe aT 


N 
S (ži +9) 
" t (7.54) 
3j (Gi +g) yi — (zi — Pe) i) 
Py = i=l x 
> (Zi +49) 
ici 


图 7.22 多 质点 的 机 器 人 模型 


258 4l 仿 人 机 器 人 建 模 与 控制 


X (7.54) 虽然 能 准确 计算 ZMP， 但 计算 流程 烦琐 。 在 一 些 场合 ， 需 要 更 简化 的 算式 ， 来 
表述 机 器 人 运动 状态 与 ZMP 的 关系 。 于 是 进一步 把 机 器 人 简化 为 单个 质点 来 表示 ， 如 图 7.23 所 
IW ENSA (7.54) 中 的 值 为 1， 即 可 得 到 单质 点 模型 下 的 ZMP 计算 公式 : 


(7.55) 


图 7.23 单质 点 的 机 器 人 模型 


如 有 果 基 于 单质 点 模型 ， 对 机 器 人 运动 状态 做 出 进一步 的 假设 ， 如 机 器 人 运动 时 质心 高 度 不 
变 和 机 器 人 在 水 平地 面 运动 , 则 可 以 使 得 式 (7.55) 中 及 ps 等于零， 从 而 进一步 简化 公式 。 基 
于 这 些 假设 的 运动 模型 已 经 在 双 足 机 器 人 的 步 态 控制 中 被 广泛 使 用 ， 我 们 在 7.3 节 详 细 讨 论 。 


73 ”基于 线性 倒立 摆 的 双 足 步 态 生成 


本 节 继 续 探讨 7.2.4 节 提 到 的 单质 点 模型 ， 通 过 添加 额外 的 限制 来 简化 式 (7.55)， 用 其 来 进 
行 双 足 步 态 质心 运动 轨迹 规划 。 使 用 线性 倒立 摆 规 划 质 心 轨迹 时 ，ZMP 集中 于 支撑 杆 末 端 ， 对 
应 于 支撑 脚 的 脚掌 中 心 ， 这 样 可 以 得 到 理论 上 稳定 的 运动 轨迹 。 


7.3.1 ”质心 轨迹 生成 


倒立 摆 模 型 CInverted Pendulum Model, IPM) 由 一 个 无 质量 的 支撑 杆 和 一 个 位 于 支撑 杆 项 端 
的 质点 (Center of Mass, COM) 构成 。 机 器 人 的 行走 轨迹 分 别 由 冠状 面 和 矢 状 面 的 倒立 摆 轨 迹 
组 合 而 成 。 对 于 一 个 固定 支撑 杆 长 度 的 倒立 摆 来 说 ， 甚 质点 在 两 个 平面 上 的 运动 方程 是 耦合 的 ， 
很 难 求解 。 通 过 引入 运动 过 程 中 质心 高 度 H 不 变 的 约束 ， 可 以 使 得 两 个 运动 方程 变 得 独立 。 这 
就 是 线性 倒立 摆 (Linear Inverted Pendulum Model, LIPM)， 如 图 7.24 所 示 。 在 实际 的 机 器 人 控制 
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中 ， 保 持 质心 固定 的 高 度 不 变 并 不 是 一 个 很 严格 的 限制 ， 甚 至 还 有 让 安装 于 头 部 的 相机 拍摄 更 
平稳 的 优势 。 通 过 在 运动 过 程 中 改变 腿 伸 直 的 幅度 可 以 实现 保持 固定 的 质心 高 度 。 


而 7 E 


724 ”二 维 平面 下 的 线性 倒立 摆 运 动 


线性 倒立 摆 的 运动 方程 导出 如 下 ， 支 撑 杆 顶端 质点 受到 竖 直 方向 重力 mg 作用 ， 支 撑 点 受 

到 地 面 支 撑 力 的 作用 。 整 个 系统 在 绕 支 撑 点 位 置 会 受到 合 扭矩 7 — mgz， 其 中 zw 为 质心 与 支撑 

点 的 水 平面 距离 。 当 支撑 杆 的 长 度 会 时 刻 智 能 变化 ， 保 持 质心 始终 位 于 同一 高 度 时 ， 此 力矩 会 
在 水 平方 向 上 对 质心 加 速 ， 加 速 力 为 局 =7+/ 互 ， 此 时 有 

ï= = =s% (1.56) 

该 式 表 明 ， 线 性 倒立 摆 上 质点 的 运动 趋势 和 线性 倒立 摆 的 参数 及 本 身 状 态 有 关 。 从 数学 角度 上 
理解 ， 该 式 为 一 个 二 阶 常 微分 方程 。 求 解 其 通 解 ， 可 得 


x(t) = zo x cosh e" x t) + la x sinh e x r) (7.57) 
H 


其 中 ，9 为 重力 加 速度 ， 互 为 质心 高 度 ; E 为 倒立 摆 的 时 间 常 数 ， 本 书 中 为 简化 描述 用 w 
RS. 

从 数学 上 来 说 ， 确 定 了 某 变量 的 初 值 及 其 随时 间 变 化 的 导数 ， 则 可 以 确定 该 变量 随时 间 变 
化 的 轨迹 。 从 通 解 公 式 中 可 得 ， 在 已 知 初 值 zo 和 wo 时 ， 可 求 得 z 随时 间 七 变化 的 任意 时 刻 的 
值 。 线 性 倒立 摆 模 型 中 ，zo 和 vo 分 别 为 质点 的 初始 位 置 和 初始 速度 。 通 解 公 式 表明 ， 知 道 质 点 
初始 状态 之 后 ， 就 可 以 根据 线性 倒立 摆 模 型 求解 质点 任意 时 刻 的 状态 了 。 线 性 倒立 摆 的 微分 方 
程 表征 了 质点 的 运动 趋势 ， 其 通 解 公式 则 表达 了 质点 具体 的 运动 轨迹 。 把 式 〈7.57) 微分 ， 即 可 
得 到 质点 的 速度 运动 轨迹 : 


v(t) = zo X "E x sinh eH x t) + vo X cosh eH x ) (7.58) 


ding ee 


倒立 摆 在 三 维 空间 中 的 运动 由 冠状 面 的 侧 向 运动 和 矢 状 面 的 前 向 运动 构成 。 两 个 平面 的 运 
动 可 以 单独 地 由 倒立 摆 轨 迹 来 描述 。 两 个 方向 的 运动 合成 后 如 图 7.25 所 示 。 分 别 查看 倒立 摆 冠 
状 面 和 矢 状 面 的 轨迹 可 以 发 现 二 者 有 一 个 明显 区 别 ， 即 冠状 面 轨迹 没有 越过 零 位 置 ， 矢 状 面 轨 
迹 则 越过 了 零 位 置 。 一 般 来 说 规划 前 进 运 动 的 时 候 倒 立 摆 的 轨迹 会 这 样 分 布 。 规 划 侧 移 运 动 时 
则 冠状 面 和 矢 状 面 的 质心 轨迹 都 不 会 越过 零 位 置 ， 且 冠状 面 的 轨迹 会 是 不 对 称 的 ， 从 而 实现 一 
步 一 步 的 侧 移 。 


time sup 
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在 多 步 连续 行走 时 认为 每 个 单 足 支 撑 期 存在 一 个 线性 倒立 摆 ， 脚 掌 踩 在 线性 倒立 摆 的 末端 
位 置 。 考 虑 简单 情况 ， 运 行 完 一 个 单 足 支撑 期 后 会 进行 支撑 脚 的 瞬间 切换 ， 接 着 质心 运行 下 一 
个 倒立 摆 的 运动 轨迹 ， 各 个 倒立 摆 的 轨迹 根据 该 步 的 步行 参数 来 规划 。 

首先 确定 当前 步 的 步行 周期 TT、 双 脚 的 冠状 面 支撑 间距 D 和 双 脚 的 矢 状 面 支撑 间距 P. PN 
后 分 别 根据 式 (7.59) 和 式 〈7.60) 分 别 计算 冠状 面 、 矢 状 面 的 质心 初速 度 vo， 再 根据 每 一 步 的 
质心 初始 位 置 zo 和 式 (7.57) 计算 当前 倒立 摆 的 质心 轨迹 。 


D D wt vo . wt 
$3 = re cosh (5. zi Es sinh (5) 
—wD (: — cosh (5) 

wT 
2sinh | — 
sin ( 2 ) 


BU GE wt UD . wt 
vy = T cosh (=) + Pa sinh (2) 


Up = (7.59) 


eee M qu 


—wD (: — cosh =) 
vo = ——— 
2sinh c3 


最 后 得 到 由 多 个 线性 倒立 摆 轨 迹 拼接 而 成 的 质心 运动 轨迹 ， 如 图 7.26 所 示 。 


(7.60) 
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7.3.2 ” 足 端 轨迹 生成 


足 端 轨迹 分 为 支撑 脚 轨迹 和 摆动 脚 轨迹 ， 生 成 支撑 脚 轨迹 时 只 需 让 支撑 脚 踩 在 倒立 摆 的 支 
撑 点 保持 不 动 即 可 。 

而 对 双 足 机 器 人 的 摆动 腿 来 说 ， 摆 动 相 的 任务 是 使 足 端 尽快 从 当前 位 置 摆动 至 下 一 步 的 着 
地 位 置 ， 摆 动 的 过 程 中 要 保证 机 器 人 双 腿 不 发 生 相 互 干涉 ， 摆 动 腿 的 运动 不 会 对 机 器 人 整体 产 
生 过 大 的 冲击 。 在 摆动 相 的 任意 时 刻 内 ， 机 器 人 足 位 置 包含 4 个 变量 。 以 机 器 人 正常 站 立时 重 
心 对 地 面 的 投影 为 坐标 原点 ， 建 立 坐 标 系 ， 需 要 的 4 个 变量 为 竖 直 高 度 H, WAME r R 
状 面 位 置 y ， 围 绕 > 轴 的 旋转 角 0。 在 直线 行走 的 情况 下 ， 对 于 冠状 面 位 置 z， 只 需 设置 为 机 器 
人 正常 站 立时 的 z 值 ， 并 在 行走 过 程 中 ， 保 持 该 值 不 变 即 可 。 因 为 在 步 态 规划 中 ， 机 器 人 矢 状 
面倒 立 摆 摆 动 跨度 玉 是 可 变 的 ,而 冠状 面 的 倒立 摆 氛 动 跨度 D 是 不 变 的 , 其 值 一 直 为 机 器 人 正 
常 站 立时 双 腿 中 心 的 间距 。 

对 矢 状 面 位 置 y， 使 用 一 个 简单 的 插值 函数 来 解决 : 


F 2 
y(t) = j sin e 一 5) (7.61) 


A (7.61) 表示 摆动 腿 矢 状 面 位 置 y ZEN T/2 内 ， 由 五 /2 aE FJ2. FERRE, H 


262 SHAS ABER 


动 腿 会 有 小 突变 ， 由 此 会 对 机 器 人 整体 产生 冲击 。 但 在 机 器 人 实体 实验 中 ， 发 现在 这 样 简单 的 
规划 下 ， 双 腿 交 换 时 机 器 人 并 没有 产生 很 大 的 冲击 ， 可 以 连续 行走 。 推 测 原因 是 机 器 人 各 个 关 
节 的 执行 器 本 身 具 有 一 定 的 柔性 。 当 速度 突变 时 ， 各 关节 因为 本 身 的 柔性 能 起 到 缓冲 作用 ， 不 
会 对 机 器 人 产生 不 良 影响 。 矢 状 面 位 置 y 与 时 间 的 变化 关系 如 图 7.27 所 示 。 


0.04 

0.03 

0.02 

0.01 

"m 0 
—0.01 
—0.02 
—0.03 


= G3 03 04 05 0.6 


t/s 
图 7.27. 摆动 腿 矢 状 面 轨迹 


对 竖 直 高 度 五 ， 同 样 采用 插值 函数 确定 。 但 是 此 时 需要 注意 的 是 ， 摆 动 腿 离 地 需要 干脆 利 
落 ， 吉 免 离 地 过 程 中 脚面 与 地 面 不 平滑 的 部 分 摩擦 ， 使 得 机 器 人 受到 整体 的 旋转 力矩 而 改变 方 
向 。 摆 动 腿 着 地 时 需要 稍微 缓慢 地 接触 地 面 ， 使 得 脚 接触 地 面 时 不 会 受到 过 大 的 地 面 反 力作 用 
而 不 稳 。 因 此 ， 在 摆动 足 上 升 阶段 和 下 降 阶 段 ， 用 不 同 的 插值 函数 来 规划 。 


上 升 阶段 为 
H(t) = Hosin (=) (7.62) 
T 
下 降 阶 段 为 
H(t) = a 4 7 sin (= n z) (7.63) 


其 相对 时 间 t 的 变化 如 图 7.28 所 示 。 

围绕 z 轴 的 旋转 6， 定义 第 il 步 机 器 人 上 身 绕 z 轴 的 转角 为 0;_1， 第 i% 步 机 器 人 上 身 绕 z 
轴 的 转角 为 0;， 第 i 十 1 步 机 器 人 上 身 绕 z 轴 的 转角 为 9;4-1。 由 于 第 i 步 的 摆动 腿 就 是 第 i 一 1 
步 的 支撑 腿 ， 则 在 第 ; 步 内 ， 摆 动 腿 需 要 摆动 的 角度 为 Ab = bigi 一 0;_1， 故 规划 转角 为 


1 二 sin ( 
O(t) = 0;1 + (0:41 — 0;—1) (7.64) 


2 
由 此 规划 ， 可 以 在 一 步 开 始 和 结束 时 ， 摆 动 腿 的 转动 速度 为 零 ， 这 样 的 性 质 有 助 于 交换 支 
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撑 腿 时 保持 稳定 。 假 设 9;_1 的 值 为 零 ， 摆 动 腿 转 动 了 0.2 弧度 ， 则 其 相对 时 间 的 变化 如 图 7.29 
所 示 。 

0.03 

0.025 

0.02 


0.015 


高 度 /m 


0.01 
0.005 
TOO 03 04 05 Us 
t/s 
7.28 ”摆动 腿 高 度 变化 轨迹 


0 Od, 0:2. 903 Q4 0.6 —016 
t/s 


FA 7.29 摆动 腿 朝向 角 变 化 轨迹 


7.3.3 ”台阶 及 斜坡 地 形 的 步 态 规划 


上 文 介 绍 了 平面 地 形 下 的 双 足 步 态 轨迹 规划 ， 其 核心 是 利用 线性 倒立 摆 模 型 得 到 支撑 腿 和 
躯干 的 运动 轨迹 。 同 时 手动 规划 摆动 脚 的 轨迹 ， 使 摆动 脚 从 当前 步 的 落脚 点 摆动 至 下 一 步 的 落 
脚 点 。 人 台阶 和 和 斜坡 地 形 与 上 述 情况 的 区 别 有 两 个 方面 。 其 一 是 线性 倒立 摆 模 型 有 一 个 最 重要 的 
假设 , 即 在 单 足 支撑 期 ， 机 器 人 躯 于 质心 高 度 不 变 ， 速 度 为 零 ; 其 二 是 摆动 运动 的 起 点 和 终点 高 
度 不 一 致 了 ， 且 终点 的 高 度 与 落脚 姿态 可 能 是 已 知 的 ， 也 可 能 是 未 知 的 。 对 于 第 一 个 区 别 ， 需 
要 在 规划 台阶 和 斜坡 步 态 时 ， 规 划 躯 干 上 升 或 下 降 的 轨迹 ， 且 上 升 轨 迹 需 要 规划 在 单 步 周期 内 
机 器 人 不 容易 失 稳 的 时 间 段 。 对 于 第 三 个 区 别 ， 则 需 把 摆动 腿 轨迹 的 末端 姿态 设 为 变量 ， 通 过 
视觉 及 压 感 检 测 等 方式 ， 得 到 落脚 点 姿态 ， 来 实时 规划 摆动 腿 轨 迹 。 
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1. 台阶 地 形 轨 迹 规 划 

为 了 增强 运动 的 稳定 性 ,避免 在 躯干 执行 上 升 的 运动 轨迹 时 摔 倒 ， 人 台阶 步 态 的 单 步 周 期 ， 需 
要 由 双 足 支撑 相 和 单 足 支撑 相 组 成 。 在 一 步 周期 中 ， 支 撑 腿 支撑 于 当前 台阶 ， 摆 动 脚 从 上 一 个 
台阶 开始 运动 ， 摆 动 至 下 一 个 台阶 。 此 时 躯干 运动 可 以 分 为 以 下 4 部 分 的 组 合 : 

运动 1: 躯干 前 进 方向 的 运动 。 

运动 2: 躯干 由 两 腿 中 间 向 支撑 脚 方向 的 运动 。 

运动 3: 躯干 由 支撑 脚 向 两 腿 中 间 方 向 的 运动 。 

运动 4 躯干 竖 直方 向 的 运动 。 

这 4 种 运动 可 以 由 不 同 的 方式 规划 , 再 相互 全 加 。 同 时 县 加 的 时 间 分 段 也 可 以 先后 不 同 , 因 
而 造成 规划 人 台阶 步 态 时 具有 非常 多 样 化 的 选择 。 而 不 同 的 运动 全 加 方式 ， 得 到 的 台阶 步 态 运 动 
结果 ， 在 稳定 性 、 流 畅 性 等 方面 有 很 大 的 不 同 。 其 中 运动 4 对 行走 稳定 性 的 影响 巨大 。 因 为 在 
线性 倒立 摆 模 型 中 ， 有 一 个 重要 假设 ， 即 认为 在 机 器 人 行走 过 程 中 , 躯干 质心 高 度 保持 不 变 ; 而 
实际 过 程 中 ， 为 了 完成 台阶 步 态 ， 必 须要 有 运动 4。 

经 实验 验证 ， 表 7.1 所 到 的 儿 种 轨迹 规划 组 合 都 是 可 行 的 。 


表 7.1 轨迹 规划 组 合 


组 合 基于 线性 倒立 摆 模 型 规划 基于 梯形 速度 曲线 规划 

组 合 1 运 功 1、 运 动 2、 运 动 3 运动 4 

组 合 2 运 功 1、 运 动 3 运动 2、 运 动 4 

组 合 3 运 功 1、 运 动 2 运动 3、 运 动 4 

组 合 4 运 功 1 运动 2、 运动 3、 运 动 4 

组 合 5 运 功 1、 运 动 2、 运 动 3、 运 动 4 


因为 运动 4 的 特殊 性 ， 导 致 运动 1、 运 动 2、 运动 3 与 运动 4 有 两 种 不 同 的 登 加 方式 ， 使 用 
以 时 间 为 横 轴 的 时 序 图 来 表示 ， 如 图 7.30 和 图 7.31 所 示 ， 其 中 箭头 方向 为 时 间 轴 。 

除了 躯干 轨迹 规划 ,还 需要 进行 摆动 腿 的 轨迹 规划 。 人 台阶 步 态 摆动 腿 运动 规划 示意 如 图 7.32 所 
示 ， 图 中 Olzz 为 支撑 腿 所 在 平面 ，5 为 台阶 长 度 ，D 为 台阶 高 度 ， 摆 动 腿 从 上 一 个 台阶 运动 至 
T—^f. 


730 运动 登 加 方式 1 
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7.31 运动 合 加 方式 2 


摆动 腿 在 与 O1xz 平行 的 平面 上 运动 ， 其 轨迹 可 以 由 一 段 上 一 台阶 落脚 点 至 下 一 台阶 落脚 
点 的 斜 线 与 一 拱 形 曲 线 的 倒 加 。 使 用 不 同 的 拱 形 曲线 具有 不 同 的 优势 ; 

sin 函数 曲线 : 形状 不 可 调节 ， 加 减速 过 程 不 柔顺 。 

择 线 :形状 不 可 调节 ， 加 减速 过 程 柔 顺 。 

样 条 曲线 : 形状 可 调节 ， 高 次 样 条 曲线 采样 点 较 多 。 

贝 塞 尔 曲线 : 形状 可 调节 ， 加 减速 过 程 柔 顺 ， 取 样 点 的 轨迹 坐标 不 直观 。 


7.32 ”台阶 步 态 摆动 腿 运 动 规划 


本 文 最 终 采 用 贝 塞 尔 曲 线 的 方式 来 规划 足 端 位 置 曲线 ， 同 时 在 跨越 较 高 的 台阶 时 ， 分 段 规 
划 足 端 〈pitch) 角 的 运动 轨迹 ， 避 免 脚掌 与 台阶 产生 干涉 。 

贝 塞 尔 曲线 (Bézier curve)， 又 称 贝 效 曲线 或 贝 济 埃 曲 线 ， 是 应 用 于 二 维 图 形 应 用 程序 的 数 
学 曲线 。 一 般 的 矢量 图 形 软件 通过 它 来 精确 画 出 曲线 ， 贝 赛 尔 曲线 由 线段 与 节点 组 成 ， 节 点 是 
可 拖 动 的 文 点 ， 线 段 像 可 伸缩 的 皮 筋 。 贝 塞 尔 曲线 主要 由 起 始点 、 终 止 点 《也 称 错 点) 控制 点 
构成 ， 通 过 调整 控制 点 ， 贝 塞 尔 曲线 的 形状 会 发 生变 化 ， 如 图 7.33 所 示 。 


oP; 


Po oP, 


A733 贝 塞 尔 曲 线 示意 图 


图 7.33 中 ， 点 Po 为 起 点 ， 点 P 为 控制 点 ， 点 已 为 终止 点 。 当 控制 点 前 后 上 下 移动 时 ， 整 
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个 曲线 的 形状 会 随 着 发 生变 化 。 因 此 ， 当 脚掌 与 台阶 表面 可 能 产生 干涉 时 ， 可 以 手动 调整 Pi 点 
的 位 置 ， 使 得 摆动 腿 的 运动 轨迹 后 移 或 者 上 抬 ， 避 开 人 台阶 表面 。 
本 书 使 用 二 阶 贝 塞 尔 曲线 来 生成 摆动 腿 轨 迹 ， 公 式 如 下 : 


B(t) = (1 — t? Ps + 2t(1 — t) P, TP t € [0,1] (7.65) 


其 中 , t 为 贝 塞 尔 曲 线 的 从 0 到 1 的 描述 参数 ，B(t) 为 贝 塞 尔 曲 线 上 各 点 的 位 置 ， 其 为 二 维 列 向 
Ht, Po. Pi. Po 分 别 为 起 点 、 控 制 点 和 终止 点 的 位 置 坐标 ， 也 是 三 维 列 向 量 。 

经 实验 验证 发 现 ， 规 划 方 式 越 接近 更 平 衡 ， 则 机 器 人 行走 越 稳定 ， 但 运动 学 限制 则 会 越 大 ， 
导致 能 跨 过 的 台阶 高 度 降低 , 机 器 人 届 腿 的 幅度 变 大 。 同 时 , 单 步 周 期 需要 调 大 ， 且 加 大 双 足 支 
撑 期 的 时 间 占 比 ， 和 否则 会 容易 失 稳 摔 倒 。 而 规划 方式 越 动 态 ， 则 机 器 人 受到 的 运动 学 限制 越 小 ， 
行走 越 流畅 ， 但 稳定 程度 会 下 降 ， 更 容易 受到 电压 波动 、 地 面 不 平整 等 偶然 因素 影响 而 摔 倒 。 

2. 斜坡 地 形 轨迹 规划 

对 于 斜坡 地 形 的 轨迹 规划 ， 基 于 线性 倒立 皖 模 型 有 以 下 两 种 可 行 的 规划 方式 。 

一 种 是 类 似 于 台阶 地 形 规 划 ， 考 虑 当 行 走 步 幅 固定 时 ， 每 两 个 落脚 点 之 间 的 高 度 差 是 固定 
的 ， 此 时 可 以 把 斜坡 当成 台阶 地 形 处 理 ， 但 是 需要 把 脚掌 运动 轨迹 的 pitch 角 设 为 倾斜 的 ， 以 便 
和 斜坡 保持 良好 接触 ， 如 图 7.34 所 示 ， 其 中 空 自 圆 为 通关 节 、 膝 关节 和 躁 关节 ， 实 心 圆 点 为 脚掌 
结构 示意 。 


Qa 
734 ”斜坡 地 形 行走 示意 图 


另 一 种 是 类 似 于 平面 地 形 。 直 接 按照 平面 地 形 规划 躯干 及 双 腿 的 运动 轨迹 ， 然 后 对 z 轴 方 
向 ， 再 爱 加 坡度 和 斜坡 一 致 的 倾斜 直线 运动 轨迹 。 这 样 处 理 躯 干 轨 迹 和 足 端 轨迹 之 后 ， 再 把 脚 
掌 运动 轨迹 的 pitch 角 设 为 倾斜 ， 倾 斜 幅度 和 和 斜坡 坡度 一 致 。 设 斜坡 坡度 为 w， 则 对 躯干 和 双 腿 
可 以 使 用 同一 个 公式 实时 计算 z 轴 方向 需要 又 加 的 高 度 。 式 (7.66) 中 为 某 个 时 刻 的 局 部 坐 
标 系 下 的 额外 县 加 高 度 ，z 为 某 个 时 刻 的 按照 平面 步 态 规划 的 躯干 或 足 端 在 局 部 坐标 系 下 的 m 
方向 的 坐标 值 。 

\ Hı = x tana (7.66) 
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经 实验 该 种 方法 可 以 稳定 地 完成 下 坡 行走 ， 但 在 上 坡 行走 时 会 有 随机 的 摔 倒 现象 出 现 。 另 
外 ， 其 行走 效果 较为 流畅 ， 步 行 周 期 可 以 接近 平面 步 态 的 步行 周期 。 

当 要 求 稳定 的 上 坡 步 态 规划 时 ， 可 以 使 用 第 一 种 方法 ， 类 比 规划 台阶 步 态 来 规划 斜坡 步 态 。 
由 此 可 以 实现 稳定 的 上 坡 行走 。 


7.4 机 器 人 静 步 态 实践 


双 足 机 器 人 行走 控制 策略 中 最 经 典 的 是 “静态 步行 ”， 这 种 策略 的 特点 是 : 机 器 人 步行 的 过 
程 中 ， 重 心 在 地 面 上 的 投影 〈 以 下 简称 重心 投影 ) 始终 位 于 支撑 多 边 形 内 。 其 具体 过 程 可 分 为 
两 个 阶段 。@ 双 足 支撑 期 重心 转移 : 在 初始 姿态 时 ， 重 心 位 于 两 脚 之 间 ， 首 先 重 心 投影 应 该 转 
移 至 支撑 脚 〈 任 意 选 定 一 只 脚 为 支撑 脚 ， 则 另 一 只 脚 为 摆动 脚 ) 的 支撑 多 边 形 内 ， 这 段 时 间 称 
为 “ 双 足 支撑 期 "。 双 足 支 撑 期 内 重心 移动 ， 双 脚 不 动 。@ 单 足 支撑 期 摆动 脚 迈步 : 待 重心 移动 
完成 后 ， 摆 动 脚 便 开 始 向 前 迈步 ， 脚 掌 离开 地 面 到 脚掌 落地 ， 这 段 时 间 称 为 单 足 支 撑 期 。 单 足 
支撑 期 内 ， 重 心 以 及 支撑 脚 不 动 ， 摆 动 脚 先 抬 脚 再 落脚 。 待 摆动 脚 落地 后 ， 再 次 转移 重心 ， 将 
重心 投影 从 支撑 脚 转移 至 摆动 脚 支 撑 多 边 形 内 ， 原 摆动 脚 变 成 新 的 支撑 脚 ， 原 支撑 脚 变 成 新 的 
摆动 脚 ， 新 的 摆动 脚 按 照 新 规划 的 轨迹 向 前 迈 一 步 ， 如 此 往复 循环 ， 就 形成 了 静态 步行 。 


7.4.1 五 次 样 条 插值 


在 移动 重心 位 置 之 前 ， 首 先 得 规划 好 重心 运动 轨迹 ， 因 此 就 需要 利用 到 五 次 样 条 插值 ， 五 
次 样 条 曲线 保证 了 位 置 、 速 度 、 加 速度 的 连续 。 基 本 方程 形式 为 五 次 多 项 式 : 


q(t) = qo + ai(t — to) + aa(t — to)? + az(t — to)? +aa(t — to)^ + as(t — to)? (7.67) 


其 中 ,如 为 初始 时 刻 ; t 为 终止 时 刻 ， t 为 初始 时 刻 与 终 正 时 刻 之 间 的 在 一 时 刻 。 在 给 定 下 面 的 
初始 条 件 后 ， 即 可 求 得 多 项 式 系数 ， 从 而 可 以 得 到 每 个 时 刻 的 位 置 。 


q(t) = qo, q(t1) = q16(t) = go, (ti) = 1 4(E) = qo, ä(t1) = qı (7.68) 


以 下 是 我 们 计算 好 的 系数 矩阵 ， 在 引用 下 面 的 函数 时 ， 需 要 创建 两 个 六 维 向 量 ， 第 一 个 向 
Æ VectorA 装 有 初始 点 和 末端 点 的 位 置 、 速 度 和 加 速度 ， 第 二 个 向 量 VectorB 装 有 五 次 多 项 式 
的 系数 。 在 引用 第 一 个 函数 时 ， 需 要 给 VectorA 赋值 ， 同 时 需要 给 定 所 规划 的 轨迹 的 运动 时 间 ， 
第 一 个 函数 计算 出 的 结果 存 入 VectorB 。 第 二 个 函数 的 输入 为 VectorB 以 及 运动 周期 中 的 某 个 时 
刻 ， 第 二 个 函数 计算 出 的 结果 为 运动 周期 中 每 个 时 刻 对 应 的 轨迹 上 的 相应 位 置 。 
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void PositionBasedController: :CubicSplineInit (Eigen: :Vector6d *In, Eigen::Vector6d * 
Out, double t) 
{ 
Matrix<double, 6, 6» m6X6; 
m6X6<< 1, 0, 0, 0, 0, 0, 
， pow(t,2), pow(t,3), pow(t,4), pow(t,5), 
ays 00, 200 QOS 


t 
1 
1, 2*t, 3*pow(t,2), 4*pow(t,3), 5*pow(t,4), 
0, 2, 0, 0, 0, 
0, 2, 6*t, 12*pow(t,2), 20*pow(t,3) ; 

(*Out) = m6X6.inverse()*(*In); 
} 
double PositionBasedController: :CubicSpline(Eigen::Vector6d *In, double t) 
{ 

double s = (*In)(0) + (*In)(1)*t + (*In) (2)*pow(t,2) + (*In) (3)*pow(t,3) + (#In) (4) 

*pow(t,4) + (*In)(5)*pow(t,5); 
return s; 


} 


7.4.2 ”实现 机 器 人 双 足 支撑 情况 下 的 重心 位 置 移动 


由 于 双 足 机 器 人 在 初始 姿态 时 ， 两 脚 平 齐 ， 重 心 投影 位 于 两 脚 之 间 ， 因 此 在 第 一 步 规划 时 ， 
需要 将 重心 投影 移动 到 支撑 脚 的 支撑 多 边 形 内 ， 这 里 需要 注意 的 是 在 对 重心 位 置 进行 规划 的 时 
候 ， 我 们 是 分 别 在 x 方向 〈 正 前 方 ) 和 yy 方向 (左手 方向 ) 规划 ， 对 于 z 方向 〈 正 上 方 ) 是 维 
持 恒定 高 度 不 变 ; 因此 对 于 第 一 步 来 说 ， 仅 仅 在 y 方向 有 位 移 。 以 下 两 个 代码 块 分 别 代表 第 一 
步 重 心 的 规划 以 及 重心 的 移动 。 其 中 工 代表 步行 周期 ， 步 行 周期 包括 双 足 支撑 期 和 单 足 支撑 期 ， 
为 了 简单 起 见 ， 各 占 0.57. 


//x 方 向 无 位 黎 ， 初 始点 和 末端 点 位 置 不 变 


comx_in<< 0 ,0,0,0,0,0; 


CubicSplineInit (&comx_in, &comx_out, 0.5*T); 


//y 方 向 有 位 移 ， 初 始点 位 置 为 0， 示 端点 位 置 为 step_y， 在 0.5T 内 完成 运动 


Comy_in<< 0,step_y,0,0,0,0; 


CubicSplineInit(£comy in, &comy_out, 0.5*T); 
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if(timeCount <= 0.5*T) 

{ 
// 随 着 timeCount 的 增加 ， 重 心 沿 着 轨迹 运动 
com[0] = CubicSpline(£comx out,timeCount ); 
com[1] = CubicSpline(&comy out ,timeCount) ; 

}else{ 
// 超 过 0.5T 后 ， 重 心 保持 在 轨迹 末端 位 置 不 变 
com[0] = CubicSpline(&comx_out ,0.5*T) ; 
com[1] = CubicSpline(&comy_out ,0.5*T); 

} 


timeCount++; 


图 7.35 为 规划 的 重心 投影 运动 轨迹 《此 处 指 规划 了 3 步 图 中 红色 轨迹 为 第 一 步 重 心 运动 
轨迹 ， 图 中 的 4 为 重心 初始 位 置 ， 在 第 一 步 双 足 支撑 期 ， 重 心 从 A 点 运动 到 点 。 


第 
一 0.06 —0.04 —0.02 0 0.02 0.04 0.06 


图 7.35 重心 投影 轨迹 


下 面 我 们 来 看 看 具体 仿真 中 机 器 人 的 运动 状态 图 (图 7.36)。 


(a) 重心 投影 最 初 位 于 两 脚 之 间 (b) 重心 投影 位 于 左 脚 的 支撑 多 边 形 内 
7.36 ”静态 步行 -重心 转移 示意 图 
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7.4.3 ”实现 摆动 脚 轨 迹 规 划 以 及 摆动 脚 的 运行 


在 对 脚 的 位 置 规划 时 ， 分 别 对 zx 方向 和 z 方向 规划 ，y 方向 维持 恒定 值 不 变 ， 这 里 需要 注 
意 的 是 ， 在 z 方向 需要 有 两 段 规划 : 抬 脚 和 落脚 ， 待 重心 移动 完成 后 ， 右 脚 随 即 开 始 向 前 走 一 
步 ， 待 右 脚 落地 后 ， 第 一 步 便 完 成 。 以 下 两 个 代码 块 分 别 代表 第 一 步 摆动 脚 的 规划 以 及 移动 。 
// 此 处 区 分 了 左 脚 和 右 脚 ， 这 里 我 们 选择 了 先 迈 右 脚 ， 因 此 当 步 数 为 奇数 时 ， 为 右 脚 逻 步 ， 步 数 为 


// 偶 数 时 ， 为 左 脚 迈步 
if(step_n/2 == 0) 


currentfootpos = 1FootPos.translation() ; 
else 


currentfootpos = rFootPos.translation() ; 


if(step_n == 1) 

footx in«« currentfootpos[0], currentfootpos[0]+step_x, 0, 0, 0, 0; 
else 

footx_in<< currentfootpos[0], currentfootpos[0]+2*step_x, 0, 0, 0, 0; 


CubicSplineInit(kfootx in, &footx out, SST); 


footup.in«« 0, step. .z, 0, 0, 0, 0; 
CubicSplineInit (&footup_in, &footup out, O.5*SST); 


footdown in«« step. z, 0, 0, 0, 0, 0; 
CubicSplineInit(&footdown in, &footdown out, 0.5*SST); 
if(foot t»-0 && foot t <= SST) 

foot[0] = CubicSpline(&footx out,foot t); 
else if(foot t» SST) 

foot[0] = CubicSpline(&footx_out ,SST) ; 
if (foot_t>=0 && foot t <= 0.5¥*SST) 

foot [2] = CubicSpline(&footup_out ,foot_t) ; 
else if(foot t > 0.5*SST && foot t <= SST) 

foot [2] = CubicSpline(&footdown out,foot t - 0.5*SST); 
else 


foot [2] = CubicSpline(&footdown out,0.5*SST); 


图 7.37 为 规划 的 脚 部 运动 轨迹 , 图 中 红色 轨迹 为 第 一 步 运 动 轨迹 , dE SE SCBERI, IRA 4 点 
运动 到 B 点 ; 
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图 7.37 脚 部 运动 轨迹 


下 面 我 们 来 看 看 具体 仿真 中 机 器 人 的 运动 状态 图 (图 7.380. 


(a) 右 脚 抬 起 (b) 右 脚 落地 
图 7.38 ”静态 步行 -摆动 脚 摆 动 示意 图 


此 后 依次 转移 重心 ， 向 前 迈步 连续 进行 ， 便 可 以 控制 机 器 人 连续 向 前 行走 。 下 面 附 上 源 代 
码 及 其 简单 解释 。 


/* 

初始 化 部 分 参数 初始 化 机 器 人 初始 姿态 

*/ 

void PositionBasedController: :PoseInit () 
{ 


timeCount = 0; 
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step_z = 0.040; 
step_y = 0.05; 
step_x = 0.1; 
foot_dis = 0.066; 
com_h = 0.33; 


T = 2.5/timeStep; 

DST = 0.5*T;//J E 3C EX 
0.5*T;// JE J£ x MEX 
step_n = 1; 

ThisStepEnd = false; 


ta 

v 

3 
Li 


comPos.translation() = Vector3d(0,0,com h); 
lFootPos.translation() = Vector3d(0,0.066,0); 
rFootPos.translation() = Vector3d(0,-0.066,0); 
updateJointAngleWithQP(comPos, lFootPos, rFootPos); 


// 调 用 规划 函数 ， 规 划 好 第 一 步 重 心 以 及 和 脚 的 运动 轨迹 
CoM_Regulate() ; 
foot Regulate(); 
5 
void PositionBasedController::Controller() 
1 
// 每 一 步 的 切换 信号 ThisStepEnd 为 false， 表 示 这 一 步 没有 执行 完 
if(ThisStepEnd == false ) 
Stepping(); 


// 这 一 步 走 完 后 ， 规 划 下 一 步 重心 以 及 脚 的 轨迹 
if(ThisStepEnd == true) 
1 

step ntt; 


CoM Regulate(); 


foot Regulate(); 
timeCount - 0; 
ThisStepEnd - false; 


cout««"step n == " ««step n ««endl; 


Y 
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void PositionBasedController: :Stepping() 


1 


Eigen::Vector3d com; com««0,0,com h; 
Eigen::Vector3d comv; comv««0,0,0; 
Eigen::Vector3d rfoot; rfoot««0,-foot dis,0; 
Eigen::Vector3d lfoot; lfoot««0, foot dis,0; 
Eigen::Vector3d foot; foot««0,0,0; 
Eigen::Vector3d . foot. ; _foot_<<0,0,0; 


// 随 着 时 间 的 增加 ， 重 心 和 脚 按照 已 经 规划 好 的 轨迹 运动 
//com 
if (timeCount <= 0.5*T) 


{ 
com[0] = CubicSpline(&comx_out,timeCount ); 
com[1] = CubicSpline(&comy_out ,timeCount) ; 
yelse{ 
com[0] = CubicSpline(£comx out,0.5*T); 
com[1] = CubicSpline(£comy out ,0.5*T); 


if (timeCount>1.0*T) 
ThisStepEnd = true; 
} 
//foot 
double foot t - timeCount - 0.50*T; 
if(foot t»-0) 
1 
if(foot t»-0 && foot t <= SST) 
foot[0] = CubicSpline(&footx out,foot t); 
else if(foot t» SST) 
foot[0] = CubicSpline(&footx out,SST); 


if(foot t»-0 && foot t <= 0.5*SST) 
foot[2] = CubicSpline(&footup out,foot t); 
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else if(foot_t > 0.5*SST && foot_t <= SST) 


foot [2] = CubicSpline(&footdown_out,foot_t - 0.5*SST); 
else 
foot[2] = CubicSpline(&footdown_out ,0.5*SST) ; 


//stepnA XH, ABIL; 奇数 时 ， 右 脚 迈 步 
if (step_n/2==0)//left foot stepping 
{ 

foot [1] = foot dis; 


1FootPos.translation() = foot; 


jelse if (step_n/2==1)//right foot stepping 
1 
foot[1] = -foot dis; 
rFootPos.translation() - foot; 
5 
} 


comPos.translation() = com; 
// 将 规划 好 的 重心 位 置 、 左 右 脚 位 置 、 给 到 逆 运动 求解 函数 ， 该 函数 求解 出 机 器 人 关节 角度 值 
updateJointAngleWithQP(comPos, lFootPos, rFootPos) ; 


timeCount++; 
} 
/* 
重心 规划 函数 
*/ 
void PositionBasedController: :CoM_Regulate() 
{ 

if(step_n == 1) 

{ 

comx_in<< 0 ,0,0,0,0,0; 


CubicSplineInit(&comx_in, &comx_out, 0.5*T); 


comy_in<< 0,step_y,0,0,0,0; 
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CubicSplineInit (&comy_in, &comy_out, 0.5*T); 
jelse 
th 
comx in«« (step n-2)*step x ,(step n-1)*step x,0,0,0,0; 


CubicSplinelnit(E£comx in, &comx out, 0.5*T); 


comy_in<<pow(-1,step_n-2)*step_y,pow(-1,step_n-1)*step_y,0,0,0,0; 
CubicSplineInit (&comy_in, &comy out, 0.5*T); 
* 

} 

/* 

脚 部 规划 函数 

*/ 

void PositionBasedController: :foot_Regulate() 

{ 


Eigen: :Vector3d currentfootpos; 


Eigen: :Vector3d anotherfootpos; 


if (step_n%2 == 0) 


currentfootpos = 1FootPos.translation() ; 


else 


currentfootpos = rFootPos.translation() ; 


if(step_n == 1) 

footx_in<< currentfootpos[0], currentfootpos[0]+step_x, 0, 0, 0, 0; 
else 

footx_in<< currentfootpos[0], currentfootpos[0]+2*step_x, 0, 0, 0, 0; 


CubicSplineInit(&footx_in, &footx_out, SST); 


footup in«« 0, step_z, 0, 0, 0, 0; 
CubicSplineInit(&footup_in, &footup out, 0.5*SST) ; 


footdown_in<< step_z, 0, 0, 0, 0, 0; 
CubicSplineInit (&footdown_in, &footdown out, 0.5*SST) ; 
} 
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图 7.39 为 连续 规划 的 质心 以 及 脚 运动 轨迹 图 ， 红色 为 重心 轨迹 ， 蓝 色 为 右 脚 轨 迹 ， 绿 色 为 左 
脚 运动 轨迹 。 


.04 
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图 7.39 ”质心 以 及 脚 部 连续 运动 轨迹 


7.5 ”机 器 人 上 楼 梯 实 践 


在 介绍 实例 之 前 ， 先 简单 介绍 一 下 贝 塞 尔 曲 线 。 贝 塞 尔 曲 线 的 应 用 得 益 于 法 国 工程 师 贝 塞 
尔 。 该 曲线 并 不 是 他 提出 的 ， 但 他 在 车 体 工 业 上 大 力 推广 并 应 用 这 种 曲线 ， 因 此 以 他 的 名 字 命 
名 。 利 用 这 种 方法 可 以 通过 很 少 的 控制 点 ， 去 生成 复杂 的 平滑 曲线 ， 因 此 在 生成 曲线 之 前 ， 首 
先 需要 选取 好 控制 点 。 腿 足 式 机 器 人 相 比 于 轮 式 机 器 人 最 大 的 优势 就 是 能 适应 复杂 的 地 形 。 本 
节 主 要 介绍 一 个 机 器 人 上 楼 梯 的 实例 ， 其 中 轨迹 规划 部 分 利用 到 了 贝 塞 尔 曲线 ， 主 要 分 为 4 个 
阶段 的 运动 。 


7.5.1 ”第 一 阶段 


这 里 我 们 选取 右 脚 先 迈 上 台阶， 在 第 一 阶段 ， 重 心 需要 向 左 侧 移动 ， 待 重心 投影 移 至 左 脚 
支撑 区 ， 右 脚 迈 上 台阶 。 在 此 期 间 ， 重 心 仅仅 只 在 Ory 平面 移动 ， 见 重心 轨迹 图 〈 图 7.44) 中 
的 4 一 B 一 C， 右 脚 在 Ozz 平面 移动 。 图 7.40 为 上 楼 梯 第 一 阶段 示意 图 。 
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(a) (b) 
图 7.40 上 楼 梯 第 一 阶段 示意 图 


if(phase == 0){ 
// 选 好 重心 轨迹 的 贝 塞 尔 曲 线 控制 点 
const Bline::Real comkeys[] = { 
0.0, 0., 0.37-0.04, 
0.0, 0.08, 0.37-0.04, 
0.03, 0.03, 0.37-0.04, 


33 

// 生 成 曲线 

comBline.build(comkeys, sizeof(comkeys)/(3*sizeof (comkeys[0]))); 
// 随 着 count 的 变化 ， 得 到 曲线 上 的 点 

comBline.getPoint(count/100. , comBline.point, comBline.tan); 


compoint = Vector3d(comBline.point.x,comBline.point.y,comBline.point.z) ; 


// 选 好 右 脚 轨迹 的 贝 塞 尔 曲线 控制 点 
const Bline::Real rfkeys[] = { 
0.0, 0., -FootD, 
0.0, 0.08, -FootD, 
step_x, 0.04, -FootD, 


step_x, 0.03, -FootD, 
3 
rfBline.build(rfkeys, sizeof(rfkeys)/(3*sizeof(rfkeys[0]))); 
if (count*timeStep<swingT/3){ 
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rfpoint = Vector3d(0.,-FootD,0.); 


5 
elsei 
rfBline.getPoint((count-33)/67. , rfBline.point, rfBline.tan); 
rfpoint = Vector3d(rfBline.point.x,rfBline.point.z,rfBline.point.y); 
} 


lfpoint = Vector3d(0.,FootD,0.); 
comPos.translation() = compoint; 
1FootPos.translation() = lfpoint; 


rFootPos.translation() = rfpoint; 


updateJointAngleWithQP(comPos, 1FootPos, rFootPos) ; 


7.5.2 “第 二 阶段 


在 第 二 阶段 ， 需 要 将 重心 转移 至 右 脚 上 方 ， 因 此 重心 在 X-Y-Z 三 个 平面 都 有 运动 ， 见 重心 
轨迹 图 (图 7.44) 中 的 C 一 D 一 BE， 待 重心 转移 至 右 脚 上 方 后 ， 左 脚 开始 运动 ， 此 时 左 脚 并 没有 
规划 相应 的 轨迹 ， 而 是 绕 着 脚尖 旋转 30*?， 这 样 是 为 了 避免 后 续 左 脚 在 运动 时 与 台阶 触 磁 。 图 
7.41 为 上 楼 梯 第 三 阶段 示意 图 。 


if(phase == 1){ 
const Bline::Real comkeys[] = f 
0.03, 0.03, 0.37-0.04, 
0.12, -0.058, 0.377-0.04, 
step_x, -0.066, 0.385-0.04, 


h 
comBline.build(comkeys, sizeof(comkeys)/(3*sizeof (comkeys[0]))); 
comBline.getPoint(count/100. , comBline.point, comBline.tan); 


compoint = Vector3d(comBline.point.x,comBline.point.y,comBline.point.z); 


IVA, IHE 
rfpoint = Vector3d(0.15,-FootD,0.03); 
lfpoint = Vector3d(0.,FootD,0.); 
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1Footxbp.translation() = Vector3d(0.09,0.,0.); 
// 待 重心 移 至 右 脚 上 方 ， 左 脚 绕 着 脚尖 旋转 
if (count>60){ 
double swingptich = (count-60)/30.*30/57.3; 
lFootPos.rotation() = sva::RotY(swingptich) ; 
J 
comPos.translation() = compoint; 
1FootPos.translation() = lfpoint; 
rFootPos.translation() = rfpoint; 
updateJointAngleWithQP(comPos, lFootPos, rFootPos,1Footxbp,rFootxbp) ; 


图 7.41 上 楼 梯 第 二 阶段 示意 图 


75.3 ”第 三 阶段 


在 第 三 阶段 ， 重心 继续 在 Oyz 平 面 移动 ， 见 重心 轨迹 图 (图 7.44) 中 的 一 F 一 G， ERE 
着 规划 好 的 贝 塞 尔 曲线 运动 ， 并 站 上 人 台阶 ， 左 脚 的 运动 分 为 两 部 分 ， 一 部 分 是 沿 着 贝 塞 尔 曲线 
运动 ， 另 一 部 分 是 将 上 一 个 阶段 旋转 的 30° 恢复 至 脚 平行 于 地 面 ， 最 终 堪 脚 站 上 人 台阶。 图 7.42 
为 上 楼 梯 第 三 阶段 示意 图 。 


if(phase == 2){ 
const Bline::Real comkeys[] = { 
0.385-0.04, -0.066, step_x, 
0.395-0.04, -0.066, step_x, 


"-—Ó eno 


0.4-0.04, -0.02, step_x, 
n 
comBline.build(comkeys, sizeof(comkeys)/(3*sizeof(comkeys[0]))) ; 
comBline.getPoint(count/100. , comBline.point, comBline.tan); 


compoint = Vector3d(comBline.point.z,comBline.point.y,comBline.point.x); 


// 右 脚 保持 不 动 
rfpoint = Vector3d(0.15,-FootD,0.03); 


/7/ 左 脚 运动 轨迹 的 贝 塞 尔 曲线 的 控制 点 
const Bline::Real lfkeys[] = + 
0.0, O., FootD, 
0.0, 0.08; FootD, 
step_x, 0.04, FootD, 
step_x, 0.03, FootD, 
3 
lfBline.build(lfkeys, sizeof(lfkeys)/(3*sizeof(lfkeys[0]1))); 
lfBline.getPoint(count/100. , lfBline.point, lfBline.tan); 
lfpoint = Vector3d(lfBline.point.x,lfBline.point.z,lfBline.point.y); 


// 左 脚 绕 y 轴 旋转 ， 恢 复 至 脚底 平行 于 地 面 

if(count>33 && count<90){ 
double swingptich = (1-(count-33)/57.)*30/57.3; 
lFootPos.rotation() = sva::RotY(swingptich) ; 

x 


comPos.translation() = compoint; 


lFootPos.translation() = lfpoint; 
rFootPos.translation() = rfpoint; 
updateJointAngleWithQP(comPos, 1FootPos, rFootPos,lFootxbp,rFootxbp); 


pea = 
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7.42 上 楼 梯 第 三 阶段 示意 图 


7.5.4 ”第 四 阶段 


此 阶段 重心 恢复 至 两 脚 之 间 ， 见 重心 轨迹 图 (图 7.44D $A G—H, 至 此 上 完 一 步 台阶 。 图 
7.43 为 上 楼 梯 第 四 阶段 示意 图 。 


图 7.43 上 楼 梯 第 四 阶段 示意 图 


if(phase == 3){ 
if(count<40){ 
compoint = Vector3d(step_x,-0.02*(1-count/40.),0.4-0.04); 
} 
else{ 
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compoint = Vector3d(step_x,0.,0.4-0.04); 
} 
comPos.translation() = compoint; 
updateJointAngleWithQP(comPos, lFootPos, rFootPos,lFootxbp,rFootxbp); 


整个 过 程 重心 的 控制 点 曲线 如 图 7.44 所 示 。 


u 


A744 重心 轨迹 图 


实际 仿真 过 程 中 运行 3 步 的 重心 轨迹 如 图 7.45 所 示 。 


u 
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图 7.45 仿真 重心 轨迹 图 


人 机 交互 


CHAPTER 8 


8.1 音频 处 理 


Roban 机 器 人 的 头 部 上 方 安装 一 款 科 大 讯 飞 的 基于 6 麦克 风 阵 列 的 模块 XFM10621( 如 图 8.1 
所 示 )， 主 要 用 于 拾 音 和 声 源 定位 。XFM10621 模块 利用 麦克 风 阵 列 的 空域 滤波 特性 ， 通 过 对 唤 
醒 人 的 角度 定位 ， 形 成 定向 拾 音 ， 并 对 波束 以 外 的 噪声 进行 抑制 ， 以 保证 较 高 的 录音 质量 。 


8.1 6 EXER 


此 麦克 风 阵 列 主要 有 以 下 特性 : 
e 6 麦克 风 环 形 麦 克 风 阵列 。 
e 360° 声 源 定位 。 
e 语音 唤醒 。 
e 回声 消除 。 
e 语音 打 断 。 
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e 去 泥 响 。 

XFM10621 核心 模块 外 置 UART 和 PC 通信 接口 ， 以 及 Line-out Al PS 音频 输出 接口 , 可 供 
开发 者 快速 体验 XFM10621 模块 远 场 拾 音 、 声 源 定 位 、 回 声 消 除 、 拾 音 模 式 切换 、 唤 醒 效 果 监 
测 等 各 项 功能 。 开 发 者 可 根据 体验 对 模块 的 相应 功能 进行 评估 ， 完 成 项 目前 期 的 调试 工作 。 图 
8.2 所 示 为 XFM10621 的 系统 结构 图 。 


回声 消除 参考 信号 


XFM10621 上 位 机 


图 8.2 系统 结构 图 


如 8.2 图 所 示 ， 模 块 接 收 外 部 的 声音 和 回声 消除 参考 信号 作为 输入 ， 进 行 降 噪 处 理 后 ， 通 过 
Line-out 和 LS 接口 输出 模拟 和 数字 音频 。 模 块 被 唤醒 后 通过 WakeUp 引 脚 进行 标志 位 输出 ， 模 
块 与 上 位 机 之 间 通 过 BC 接口 实现 控制 和 数据 传输 ，UART 接口 用 于 输出 麦克 风 阵 列 的 唤醒 消 
kk, BEA NUC 上 位 机 进行 解析 。 

与 麦克 风 相 关 的 操作 主要 在 Roban 机 器 人 的 ros_mic_arrays 节点 中 , 机 器 人 启动 后 , 可 通过 
唤醒 词 “ 灵 犀 灵 犀 ” 唤 醒 麦 克 风 ， 如 果 有 可 视 化 屏幕 ， 则 可 以 在 终端 看 到 打印 的 唤醒 信息 ， 类 
(AF: ('key word': 'lingxilingxi \n'，'score': 'xxxx', 'angle': 'xx'}， 如 图 8.3 所 示 ， 同 时 每 
次 唤醒 都 会 发 布 一 个 ROS 话题 消息 : /micarrays/wakeup. 

头 部 舵 机 转向 声 源 demo， 主 要 演示 6 麦克 风 环 形 麦 克 风 阵列 的 语音 唤醒 和 声 源 定位 功能 ， 
通过 唤醒 词 “ 灵 犀 灵 犀 ”，Roban 机 器 人 即 可 根据 声 源 方位 调整 头 部 转向 ， 始 终 朝 向 唤醒 者 。 其 
程序 文件 为 head_toward_sound.py， 文 件 路 径 为 : 
/home/lemon/robot_ros_application/catkin_ws/src/ros_actions_node/scripts/head_toward_sound.py . 


运行 方式 为 :在 终端 下 ， 首 先 通过 以 下 指令 占用 BodyHub， 使 其 可 控制 舵 机 运动 。 代 码 如 下 : 


rosservice call /MediumSize/BodyHub/StateJump 2 setStatus 


然后 来 到 demo 文件 路 径 下 ， 在 终端 运行 : 
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‘Python head_toward_sound.py 


即 可 局 动 头 部 舵 机 转向 声 源 DEMO 程序 。 


8.3 麦克 风 唤 醒 


值得 注意 的 是 ， 麦 克 风 阵列 识别 方位 角 的 精度 在 10° 左右 ， 所 以 只 能 判别 大 概 方位 。 如 果 
需要 准确 识别 ， 还 需要 和 视觉 相关 传感器 进行 融合 。 关 于 Roban 机 器 人 视觉 ， 将 在 下 个 章节 进 
行 介绍 。 


8.1.1 ”语音 识别 


在 人 际 交往 中 ， 语 言 是 最 自然 并 且 最 直接 的 方式 之 一 。 随 着 技术 的 进步 ， 越 来 越 多 的 人 也 
期 望 计算 机 能 够 具备 与 人 进行 语言 沟通 的 能 力 ， 因 此 语音 识别 这 一 技术 也 越 来 越 受 到 关注 ， 尤 
其 随 着 深度 学 习 技术 在 语音 识别 技术 中 的 应 用 ， 使 语音 识别 的 性 能 得 到 了 显著 提升 ， 也 使 语音 
识别 技术 的 普及 成 为 现实 。 

简单 来 说 ， 自 动 语音 识别 技术 其 实 就 是 利用 计算 机 将 语音 信号 自动 转换 为 文本 的 一 项 技术 ， 
如 图 8.4 所 示 。 这 项 技术 同时 也 是 机 器 理解 人 类 语言 的 第 一 个 也 是 很 重要 的 一 个 过 程 。 


-— = 


84 自动 语音 识别 技术 


语音 控制 的 基础 就 是 语音 识别 技术 ， 可 以 是 特定 人 或 者 非特 定 人 的 。 非 特定 人 的 应 用 更 为 
广泛 ， 对 于 用 户 而 言 不 用 训练 ， 因 此 也 更 加 方便 。 语 音 识 别 可 以 分 为 孤立 词 识别 、 连 接 词 识别 ， 
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以 及 大 词汇 量 的 连续 词 识别 。 对 于 智能 机 器 人 这 类 艇 入 式 应 用 而 言 ， 语 音 可 以 提供 直接 、 可 靠 
的 交互 方式 ， 因 此 语音 识别 技术 的 应 用 价值 不 言 而 喻 。 

为 了 进一步 理解 机 器 人 如 何 实现 语音 到 文字 的 转换 这 一 过 程 ， 首 先 给 出 目前 比较 主流 的 自 
动 语 音 识别 系统 的 整体 框架 ( 见 图 8.5)， 然 后 再 一 一 简要 地 对 各 部 分 进行 说 明 。 


语音 — 3| 特征 提取 | 语音 特征 词 序列 


PEL 


8&5 ”自动 语音 识别 系统 整体 框架 


当 我 们 要 对 一 段 语音 进行 识别 时 ， 首 先 需要 进行 的 是 对 语音 特征 的 提取 。 这 一 步 所 做 的 工 
作 其 实 就 是 从 输入 的 语音 信号 (时 域 信号 ) 中 提取 出 可 以 进行 建 模 的 声学 观测 特征 向 量 序列 O。 
通俗 地 解释 就 是 把 需要 识别 的 一 段 语音 进行 特征 提取 ， 之 后 得 到 一 组 可 以 表征 这 一 段 语音 的 向 
量 ， 后 续 对 语音 进行 的 一 系列 操作 都 是 基于 这 组 向 量 的 。 
在 得 到 了 这 组 观测 特征 向 量 O 之 后 ， 可 以 用 一 个 公式 来 说 明 语 音 识别 具体 是 要 做 一 个 什么 
样 的 事情 : 
W = arg max P(W |O) 


其 中 ，P(O) 是 声学 观测 的 先 验 概 率 ， 在 自动 语音 识别 过 程 中 ， 由 于 输入 的 声学 观测 特征 序列 是 
固定 的 ， 可 以 认为 上 述 公式 中 的 P(O) 是 常量 ， 因 此 P(O) 在 上 述 公式 的 最 大 化 的 过 程 中 不 起 
作用 ， 可 以 忽略 。 那 么 ， 我 们 现在 只 剩 下 P(O|W) 和 P(W) 需要 考虑 。 而 在 自动 语音 识别 系统 
整体 框架 〈 见 图 8.5) 中 的 声学 模型 和 语音 模型 分 别提 供 了 对 P(O|W) 和 P(W) 进行 计算 的 方 
法 ， 下 面 简单 介绍 声学 模型 。 

首先 是 声学 模型 ， 其 目的 是 提供 一 种 方法 ， 以 对 给 定 词 W 的 声学 观测 特征 序列 O 的 似 然 
度 进行 计算 。 (可 以 理解 成 给 定 一 个 词 WW， 然 后 计算 目前 这 个 特征 问 量 是 描述 这 个 词 的 可 能 性 
有 多 大 ， 也 就 是 计算 P(O|W))， 所 以 这 个 建 模 的 任务 就 可 以 简单 地 理解 成 对 每 个 词 建立 一 个 
描述 概率 分 布 的 模型 ， 该 模型 的 输入 是 声学 特征 向 量 ， 输 出 则 是 一 个 概率 〈 似 然 值 )， 概 率 越 高 
表示 该 声学 特征 越 可 能 表示 的 是 这 个 词 。 但 是 在 实际 的 大 词汇 量 语音 识别 任务 中 ， 如 果 对 每 个 
词 建立 一 个 模型 是 很 不 现实 的 。 因 为 词 的 数量 非常 多 ， 而 且 经 常会 有 新 词 出 现 。 为 了 解决 这 个 
问题 ， 声 学 模型 通常 不 会 直接 对 词 进行 建 模 ， 而 是 将 词 拆 成 字 词 序列 ， 对 字 词 进行 建 模 。 举 个 
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例子 ， 汉 语 中 的 汉字 有 几 万 个 ， 但 是 如 果 将 汉字 拆 分 成 音标 〈 如 “ 跑 ” 拆 分 成 pao)， 那 么 只 需 
要 用 几 十 个 音标 就 可 以 表示 所 有 汉字 的 读音 ， 就 算 考虑 音调 ， 最 多 也 只 需要 几 百 个 音标 就 足够 
了 。 然 后 ， 对 音标 进行 建 模 ， 再 将 其 拼接 成 汉字 ， 就 可 以 得 到 我 们 需要 的 P(O[W) 同时 却 大 大 
减少 了 建 模 的 数量 。 因 此 ， 目 前 主流 的 声学 建 模 方法 是 采用 对 语音 的 基本 单位 一 一 音 子 进行 建 
模 〈 音 子 与 音标 有 区 别 ， 但 可 以 利用 音标 对 音 子 的 概念 进行 理解 )。 

1. 语音 识别 概述 

语音 识别 技术 最 早 可 以 追溯 到 20 世纪 50 年 代 ， 是 试图 使 机 器 能 “ 听 懂 ”人 类 语音 的 技术 。 
按照 目前 主流 的 研究 方法 ， 连 续 语 音 识 别 和 孤立 词语 音 识别 采用 的 声学 模型 一 般 不 同 。 孤 立 词 
语音 识别 一 般 采 用 动态 时 间 规 (Dynamic Time Warping, DTW) 算法 。 连 续 语 音 识别 一 般 采 用 隐 
马尔 可 夫 模 型 (Hidden Markov Model, HMM) 模型 或 者 HMM 与 人 工 神经 网 络 〈Artificial Neural 
Network，ANN) 相 结 合 。 

语音 的 能 量 来 源 于 正常 呼 气 时 肺 部 呼出 的 稳定 气流 ， 喉 部 的 声带 既是 阀门 ， 又 是 振动 部 件 。 
语音 信号 可 以 被 看 作 一 个 时 间 序 列 ， 可 以 由 HMM 进行 表征 。 语 音信 号 经 过 数字 化 及 滤 品 处理 
之 后 ， 进 行 端 点 检测 得 到 语音 段 。 对 语音 段 数 据 进行 特征 提取 ， 语 音信 和 号 就 被 转换 成 为 一 个 向 
量 序列 ， 作 为 观察 值 。 在 训练 过 程 中 ， 观 察 值 用 于 估计 HMM 的 参数 。 这 些 参数 包括 观察 值 的 
概率 密度 函数 及 其 对 应 的 状态 ， 以 及 状态 转移 概率 等 。 当 参数 估计 完成 后 ， 估 计 出 的 参数 即 用 
于 识别 。 此 时 经 过 特征 提取 后 的 观察 值 作 为 测试 数据 进行 识别 ， 由 此 进行 识别 准确 率 的 结果 统 
计 。 语 音 训练 及 识别 的 结构 框图 如 图 8.6 所 示 。 


语音 


语音 
— 


8.6 语音 训练 及 识别 的 结构 框图 


识别 输出 


2. 端点 检测 

找到 语音 信号 的 起 止 点 ， 从 而 减 小 语音 信号 处 理 过 程 中 的 计算 量 ， 是 语音 识别 过 程 中 一 个 
基本 而 且 重要 的 问题 。 端 点 作为 语音 分 割 的 重要 特征 ， 其 准确 性 在 很 大 程度 上 影响 系统 识别 的 
性 能 。 

能 零 积 定义 ， 一 帧 时 间 范 围 内 的 信号 能 量 与 该 段 时 间 内 信和 号 过 零 率 的 乘积 。 

能 零 积 门限 检测 算法 可 以 在 不 丢失 语音 信息 的 情况 下 ， 对 语音 进行 准确 的 端点 检测 〈 见 图 
8.7)， 经 过 450 个 孤立 词 (数字 “0 一 9”) 测试 准确 率 为 98% 以上， 经 该 方法 进行 语音 分 制 后 的 
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语音 ， 在 进入 识别 模块 时 识别 正确 率 达 95% 

当 语 音 带 有 呼吸 噪声 ， 或 周围 环境 出 现 持 续 时 间 较 短 而 能 量 较 高 的 噪声 ， 或 者 持续 时 间 长 
而 能 量 较 弱 的 噪声 时 ， 能 零 积 门限 检测 算法 就 不 能 对 这 些 噪声 进行 滤 除 ， 进 而 被 判 作 语音 进入 
识别 模块 ， 导 致 误 识 别 。 


Input signal 


4000 
ui 
kg 2000 
m 
T oO 
a 

—2000 

0.5 1 1.5 2 2.5 
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Example VAD output 


1 1.5 
时 间 /s 


8. ”端点 检测 


3. Roban 机 器 人 语音 识别 

语音 识别 是 目前 人 工 智能 的 一 个 热点 ， 也 是 一 个 难点 。 例 如 ， 连 续 多 轮 对 话 、 远 场 大 词汇 
识别 、 声 纹 识别 等 都 是 目前 研究 的 热点 。 

下 面 以 百度 识别 方案 为 例 讲解 和 演示 Roban 机 器 人 语音 识别 相关 内 容 。 

首先 通过 pip (如 果 没 有 安装 pip， 请 先 安装 pip ) 安装 百度 语音 Python SDK: 


pip install baidu-aip 


安装 完成 后 ， 需 要 申请 一 个 百度 语音 识别 接 入 的 开发 者 账号 ， 网 址 为 : 
https://ai.baidu.com/tech/speech， 目 的 是 获取 App ID, API Key. Secret Key。 然 后 录制 一 个 名 
为 test.wav 的 录音 文件 ， 录 制 内 容 是 : “这 是 一 个 百度 语音 识别 的 测试 ”。 接 下 来 创建 一 个 名 
W baidu_asr.py 的 文件 以 开始 语音 识别 请 求 : 


# -*- coding:utf-8 -*- 


from aip import AipSpeech 
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"nu 你 的 AppID AK SK """ 

* APP ID = :你 的 App ID' 

* API KEY = ' 你 的 API Key' 

# SECRET KEY = ' 你 的 Secret Key' 


client = AipSpeech(APP_ID, API_KEY, SECRET_KEY) 


# 读 取 文 件 
def get file content(file, path): 
with open(file path, 'rb') as fp: 
return fp.read() 
# 识别 本 地 录音 文件 
res = client.asr(get_file_content('./test.wav'), 'wav', 16000, { 
'dev.pid': 1537, 4 默认 1537( 普 通话 输入 法 模型 ) 
}) 


# 返回 的 文字 结果 在 "key" 为 "result" 中 ， 直 接 取出 来 打印 显示 
text result = ''.join(res['result']) 


print(text result) 


到 baidu_asr.py 路 径 下 执行 : 


python baidu_asr.py 


可 见 终端 打印 出 :“ 这 是 一 个 百度 语音 识别 的 测试 ”。 


目前 百度 提供 示例 演示 代码 供 参 考 ， 地 址 为 https://github.com/Baidu-AIP/speech-demo。 


8.1.2 ”语音 合 


语音 合成 (Test to Speech, TTS) 是 将 文字 转换 为 语音 的 一 种 技术 ， 类 似 于 人 类 的 嘴巴 ， 通 
过 不 同 的 音色 说 出 想 表 达 的 内 容 。 在 语音 合成 技术 中 ， 主 要 分 为 语言 分 析 部 分 和 声学 系统 部 分 ， 
也 称 为 前 端 部 分 和 后 端 部 分 。 其 中 ， 语 言 分 析 部 分 主要 根据 输入 的 文字 信息 进行 分 析 ， 生 成 对 
应 的 语言 学 规格 书 ， 想 好 该 怎么 读 ; 声学 系统 部 分 主要 根据 语音 分 析 部 分 提供 的 语音 学 规格 书 ， 


生成 对 应 的 音频 ， 实 现 发 声 的 功能 。 
1. 语言 分 析 部 分 
语言 分 析 部 分 的 流程 如 下 ， 可 以 简单 描述 出 语言 分 析 部 分 的 主要 工作 。 
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(1) 文本 结构 与 语种 判断 。 当 需要 合成 的 文本 输入 后 ， 先 要 判断 是 什么 语种 ， 如 中 文 、 英 
文 、 藏 语 、 维 语 等 ， 再 根据 对 应 语种 的 语法 规则 ， 把 整 段 文字 切 分 为 单个 的 句子 ， 并 将 切 分 好 
的 句子 传 到 后 面 的 处 理 模 块 。 

(2) 文本 标准 化 。 在 输入 需要 合成 的 文本 中 ， 有 阿拉 伯 数 字 或 字母 ， 需 要 转换 为 文字 。 根 据 
设置 好 的 规则 ， 使 合成 文本 标准 化 。 例 如 , “请问 您 是 尾 号 为 8967 的 机 主 吗 ?” 其 中 的 “8967” 
为 阿拉 伯 数 字 ， 需 要 转换 为 汉字 “ 八 九 六 七 ”% 这 样 便于 进行 文字 标 音 等 后 续 的 工作 ;再 如 ， 对 
于 数字 的 读 法 ， 上 面 的 “8967” 为 什么 没有 转换 为 “ 八 千 九 百 六 十 七 ” 呢 ? 因为 在 文本 标准 化 
的 规则 中 ， 设 定 了 “ 尾 号 为 + 数字 ”的 格式 规则 ， 这 种 情况 下 数字 按照 这 种 方式 播报 。 这 就 是 
文本 标准 化 中 设置 的 规则 。 

(3) 文本 转 音 素 在 汉语 的 语音 合成 中 , 基本 上 是 以 拼音 对 文字 标注 的 ， 所 以 需要 把 文字 转 
换 为 相对 应 的 拼音 ， 但 是 有 些 字 是 多 音字 ， 怎 么 区 分 当前 是 哪个 读音 ， 就 需要 通过 分 词 、 词 性 
句法 分 析 ， 判 断 当 前 是 哪个 读音 ， 并 且 是 几 声 的 音调 。 

例如 ,“ 南 京 市 长 江 大 桥 ” 为 “nan2jinglshi4zhang3jianglda4qiao2” 或 者 “nan2jing1shi4chang2- 
jiang1da4qiao3 ”。 

(D 句 读 韵 律 预 测 。 人 类 在 语言 表达 的 时 候 总 是 附带 着 语气 与 感情 ， 语 音 合 成 的 音频 是 为 
了 模仿 真实 的 人 声 ， 所 以 需要 对 文本 进行 韵律 预测 。 例 如 ， 什 么 地 方 需要 停顿 ， 停 顿 多 久 ， 哪 
个 字 或 者 词语 需要 重读 ， 哪 个 词 需要 轻 读 等 ， 实 现 声音 的 高 低 曲 折 ， 抑 扬 顿 挫 。 

2. 声学 系统 部 分 

声学 系统 部 分 目前 主要 有 3 种 技术 实现 方式 : 波形 拼接 语音 合成 、 参 数 语音 合成 和 端 到 端 
语音 合成 。 

1) 波形 拼接 语音 合成 技术 

通过 前 期 录制 大 量 的 音频 ， 尽 可 能 全 地 覆盖 所 有 的 音节 音素 ， 基 于 统计 规则 的 大 语料库 拼 
接 成 对 应 的 文本 音频 ， 所 以 波形 拼接 语音 合成 技术 通过 已 有 库 中 的 音节 进行 拼接 ， 实 现 语音 
合成 的 功能 。 此 技术 需要 大 量 的 录音 ， 录 音量 越 大 ， 效 果 越 好 ， 一 般 做 得 好 的 音 库 ， 录 音量 在 
50h 以 上 。 波 形 拼 接 语音 合成 如 图 8.8 所 示 。 


8.8 波形 拼接 语音 合成 
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优点 : 音质 好 ， 情 感 真实 。 

缺点 : 需要 的 录音 量 大 ， 履 盖 要 求 高 ， 字 间 协 同 过 渡 生 硬 ， 不 平滑 ， 不 是 很 自然 。 

2) 参数 语音 合成 技术 

参数 语音 合成 技术 主要 是 通过 数学 方法 对 已 有 录音 进行 频谱 特性 参数 建 模 ， 构 建文 本 序列 
映射 到 语音 特征 的 映射 关系 ， 生 成 参数 合成 器 。 所 以 ， 当 输入 一 个 文本 时 ， 先 将 文本 序列 映射 
出 对 应 的 音频 特征 ， 再 通过 声学 模型 “ 声 码 器 ) 将 音频 特征 转换 为 人 能 听 得 懂 的 声音 。 参 数 语 
音 合 成 如 图 8.9 所 示 。 

优点 ; 录音 量 小 ， 可 多 个 音色 共同 训练 ， 字 间 协 同 过 渡 平 滑 、 自 然 等 。 

缺点 : 音质 没有 波形 拼接 的 好 ， 机 械 感 强 ， 有 杂音 等 。 
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3) 端 到 端 语音 合成 技术 

端 到 端 语 音 合成 技术 是 目前 比较 流行 的 技术 ， 其 通过 神经 网 络 学 习 的 方法 ， 实 现 直接 输入 
文本 或 者 注音 字符 ， 中 间 为 黑 盒 部 分 ， 然 后 输出 合成 音频 ， 对 复杂 的 语言 分 析 部 分 得 到 了 极 大 
的 简化 。 所 以 ， 端 到 端的 语音 合成 技术 大 大 降低 了 对 语言 学 知识 的 要 求 ， 且 可 以 实现 多 种 语言 
的 语音 合成 ， 不 再 受 语言 学 知识 的 限制 。 通 过 端 到 端 合成 的 音频 ， 效 果 得 到 进一步 的 优化 ， 声 
音 更 加 贴近 真人 。 端 到 端 语音 合成 如 图 8.10 所 示 。 


文本 或 注音 字符 —MEMEe — oon 


图 8.10 端 到 端 语音 合成 


优点 : 对 语言 学 知识 要 求 降低 ， 合 成 的 音频 拟人 化 程度 更 高 ， 效 果 好 ， 录 音量 小 。 

缺点 : 性 能 大 大 降低 ， 合 成 的 音频 不 能 人 为 调 优 。 

目前 的 语音 合成 技术 已 经 应 用 于 各 种 场景 ， 是 较 成 熟 、 可 落地 的 产品 ， 对 于 合成 音 的 要 求 ， 
当前 的 技术 已 经 可 以 做 得 很 好 了 ， 可 满足 市 场 上 绝 大 部 分 需求 。 语 音 合成 技术 主要 是 合成 类 似 
于 人 声 的 音频 ， 其 实 当 前 的 技术 已 完全 满足 。 目 前 的 问题 在 于 不 同 场 景 的 具体 需求 的 实现 ， 例 
如 ， 不 同 的 数字 读 法 ， 如 何 智 能 地 判断 当前 场景 应 该 是 哪 种 播报 方式 ， 以 及 什么 样 的 语气 和 情 
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绪 更 适合 当下 的 场景 ， 如 何 更 好 地 区 分 多 音字 以 确保 合成 的 音频 尽 可 能 地 不 出 错 。 当 然 ， 错 误 
有 时 候 是 不 可 避免 的 ， 但 是 如 何在 容错 范围 内 ， 或 者 读 错 之 后 是 否 有 很 好 的 自学 机 制 ， 下 次 播 
报时 就 可 以 读 对 ， 具 有 自我 纠 错 的 能 力 ， 这 些 问题 可 能 是 当前 产品 化 时 遇 到 的 更 多 、 更 实际 的 
间 题 ， 在 产品 整体 设计 的 时 候 ， 这 些 问 题 是 需要 考虑 的 主要 问题 。 

3. Roban 机 器 人 语音 合 

目前 ,语音 合成 技术 已 经 获得 了 非常 快速 的 发 展 ， 市 场 上 主流 的 有 科大 讯 飞 、 百 度 、 微 软 、 
亚马逊 、 谷 歌 等 人 工 智能 公司 的 技术 。 而 我 们 通过 对 这 种 技能 的 学 习 可 以 为 日 后 的 开发 和 研究 
打下 坚实 的 基础 。 例 如 ， 科 大 讯 飞 在 2017 年 推出 了 讯 飞 实时 翻译 机 ， 而 目前 随 着 语音 技术 的 发 
展 ， 配 套 设 备 诸如 环形 麦克 风 、 智 能 拾 音 、 消 回声 芯片 校 组 等 也 雨 后 春 筹 般 地 来 到 了 生产 链 的 
世界 。 未 来 世界 将 是 一 个 语音 无 处 不 在 的 世界 ， 而 与 它 相 关 的 各 种 机 器 人 也 将 成 为 消费 者 用 户 
的 时 尚 新 定 。 

同 语音 识别 一 节 ， 以 下 使 用 百度 语音 来 讲解 、 演 示 Roban 机 器 人 语音 合成 。 

首先 安装 Python SDK。 如 果 之 前 已 安装 ， 此 步 可 省 略 。 


ip install baidu-aip 


安装 完成 后 ， 需 要 申请 一 个 百度 语音 合成 的 开发 者 账号 ， 网 址 为 https://ai.baidu.com/tech/ 
speechy/tts-online。 如 果 之 前 已 经 申请 ， 此 步 可 省 略 ， 随 后 需要 到 百度 控制 台 语 音 合成 领取 免费 
使 用 次 数 ， 会 有 总 量 为 5000 次 的 请 求 赠送 ， 足 够 学 习 使 用 。 

接 下 来 ， 创 建 一 个 名 为 baidu_tts.py 的 文件 用 于 语音 合成 : 


# -*- coding:UTF-8 -*- 


from aip import AipSpeech 


""" 你 的 APPID AK SK """ 

APP ID = ' 你 的 App ID' 

API KEY = ' 你 的 API Key' 
SECRET KEY = :你 的 Secret Key' 


client = AipSpeech(APP ID, API KEY, SECRET. KEY) 


result = client.synthesis(' 你 好 百度 ',，'zh', 1, { 
"DOr d, 
'vol': 5, 

n 


第 8 章 ”人 机 交互 i> 293 


# 识别 正确 ,返回 语音 二 进 制 ; 错误 ， 则 返回 qict 
if not isinstance(result, dict): 
with open('audio.mp3', 'wb') as f: 
f.write(result) 


到 baidu_tts.py 路 径 下 执行 : 


python baidu_tts.py 


成 功 请 求 可 在 baidu_tts.py 相同 路 径 下 生成 一 个 名 为 audio.mp3 的 音频 文件 。 播 放 该 文件 ， 可 听 
到 “你 好 百度 ”的 声音 。 


8.1.3 ”聊天 机 器 人 综合 应 用 


近 几 年 来 ， 人 工 智 能 发 展 火热 ， 尤 其 是 语音 识别 方面 的 落实 项 目 更 是 普遍 存在 于 人 们 的 生 
活 中 ， 像 手机 中 常见 的 语音 助手 、Siri 和 计算 机 中 的 小 娜 等 , 但 是 它们 却 很 难 做 到 私人 定制 的 效 
果 ， 即 达到 个 人 个 性 化 的 需求 ， 所 以 本 节 旨 在 搭建 一 个 基于 Roban 机 器 人 的 个 性 化 的 语音 聊天 
机 器 人 。 图 8.11 所 示 为 语音 对 话机 器 人 系统 。 一 个 完整 的 语音 对 话机 器 人 系统 通常 包括 以 下 主 
要 部 分 。 

(1) 语音 识别 (Automatic Speech Recognition，ASR)。 这 部 分 是 将 声音 转换 为 文字 的 过 程 ， 
相当 于 人 类 的 耳 灯 。 

(2) 自然 语言 理解 (Natural Language Understanding，NLU )、 对 话 管理 (Dialog Management, 
DM) 和 自然 语言 生成 (Natural-Language Generation，NLG)。 这 部 分 是 理解 和 处 理 文字 的 过 程 ， 
对 话 管理 主要 帮助 理解 多 轮 对 话 进 行 时 的 上 下 文 含义 ， 相 当 于 人 类 的 大 脑 。 
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8.1. 语音 对 话机 器 人 


(3) 语音 合成 (Text-To-Speech; TTS)。 这 部 分 是 将 文字 转换 为 语音 (朗读 出 来 ) 的 过 程 ， 相 
当 于 人 类 的 嘴巴 (与 ASR 是 相反 的 )。 
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Roban 机 器 人 中 的 一 个 语音 聊天 对 话 是 基于 开源 项 目 wukong-robot 进行 定制 修改 而 成 的 。 
wukong-robot 是 一 个 简单 、 灵 活 、 优 雅 的 中 文 语音 对 话机 器 人 /智能 音箱 项 目 ， 目 的 是 让 中 国 的 
制造 商 也 能 快速 打造 个 性 化 的 智能 音箱 。 图 8.12 所 示 为 wukong-robot 的 整体 框架 。 


HomeAssistant 


@ vo i 
Oma 


a 
psa TF 
f Emotibot — : 
v G | ASR. {© 腾讯 
Bae AnyQ PLA) £ 
: ^ X 科大 讯 飞 


(3 snowboy 
ANE IO ** Muse 随 机 


© 后 台 服 务 | "ys & 百度 
f" Me | Gus 
ms ' TTS Tu e 腾讯 = 
TTS perg 
Han TTS 


Gag) E 


842 HER 


wukong-robot 具有 如 下 特点 : 

(1) 模块 化 。 功 能 插件 、 语 音 识 别 、 语 音 合成 、 对 话机 器 人 都 做 到 了 高 度 模块 化 ， 第 三 方 插 
件 单 独 维护 ， 方 便 继承 和 开发 自己 的 插件 。 

(2) 中 文 支 持 。 集 成 百度 、 科 大 讯 飞 、 阿 里 、 腾 讯 等 多 家 中 文 语音 识别 和 语音 合成 技术 ， 且 
可 以 继续 扩展 。 

(3) 对 话机 器 人 支持 。 支持 基于 AnyQ 的 本 地 对 话机 器 人 , 并 支持 接 入 图 灵机 器 人 、Emotibot 
等 在 线 对 话机 器 人 。 

(4) 全 局 监听 ， 离 线 唤醒 。 支 持 Muse 脑 机 唤醒 及 无 接触 的 离线 语音 指令 唤醒 。 

(5) 灵活 可 配置 。 支 持 定 制 机 器 人 名 字 ， 支 持 选 择 语音 识别 和 合成 的 插件 。 

(6) 智能 家 居 。 支 持 和 MQTT、HomeAssistant 等 智能 家 居 协 议 联动 ， 支 持 语音 控制 智能 
家 电 。 
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CD 后 台 配 套 支 持 。 提 供 配 套 后 台 ， 可 实现 远程 操控 、 修 改 配置 和 日 志 查 看 等 功能 。 

(8) 开放 API。 可 利用 后 端 开放 的 API， 实 现 更 丰富 的 功能 。 

(9) 安装 简单 ， 支 持 更 多 平台 。 相 比 dingdang-robot , $ f PocketSphinx 的 离线 唤醒 方案 ， 
安装 变 得 更 加 简单 ， 代 码 量 更 少 ， 更 易于 维护 并 且 能 在 Mac 以 及 更 多 Linux 系统 中 运行 。 

1. wukong-robot 的 基本 原理 

wukong-robot 的 工作 原理 : 录音 并 获得 录音 文件 9. 百度 语音 识别 将 录音 转换 为 文字 一 图 
灵机 器 人 理解 和 处 理 并 得 到 答 语 的 文字 一 百度 语音 合成 将 答 语 文字 转换 为 音频 文件 > 通过 播 
放 器 播放 。 

具体 流程 通过 唤醒 词 “ 灵 犀 灵 犀 ” 唤醒 麦克 风 阵 列 ， 检 测 到 麦克 风 唤 醒 后 的 串口 信息 后 
进入 拾 音 , 通过 语音 端点 检测 (Voice Activity Detection, VAD) 开始 说 话 和 说 话 结束 的 状态 ， 随 
后 保存 说 话 的 录音 ， 然 后 上 传 录 音 文件 请 求 百 度 语 音 识 别 API， 将 其 转换 为 文字 ， 随 后 将 文字 
传 给 图 灵 聊 天 机 器 人 ， 并 得 到 回答 的 文字 ， 然 后 将 回答 文字 传递 给 百度 语音 合成 API， 转 换 为 
相应 的 音频 文件 ， 最 后 通过 机 器 人 的 播放 器 播放 该 音频 文件 ， 完 成 一 次 对 话 ， 随 后 ， 可 继续 与 
wukong-robot 对 话 ， 当 超过 10 s 未 检测 到 语音 时 ，wukong-robot 进入 待 唤醒 状态 。 
语音 请 求 接口 
aip.AipSpeech 
ROS 接口 


2. wukong-robot 运行 方式 
在 Roban 机 器 人 的 ROS 工作 空间 ， 运 行 下 面 的 脚本 ， 就 能 启动 Roban 机 器 人 的 聊天 对 话 功能 。 


|rosrun wukong robot wukong.py 


Roban 机 器 人 语音 演示 问答 逻辑 ; 

e 唤醒 词 RERE) : 拾取 到 唤醒 词 ， 机 器 人 随机 播报 “在 呢 !”“Hi!”“ 有 什么 事 ?”“ 我 
在 !1”“ 来 者 何人 ?” 等 。 

e 被 唤醒 后 Ss 内 没 拾 取 到 问 名: 机 器 人 随机 播报 “我 没 听 清楚 ， 再 说 一 遍 好 吗 ?”“ 我 喜欢 
洪亮 的 声音 ”“ 你 的 声音 很 温柔 ， 我 没 听 清 呢 !”“ 你 可 以 这 样 问 我 ,【 问 答 库 中 随机 播报 
一 个 问 名 六。 随后 进入 拾 音 状态 。 

e 被 唤醒 后 10s 内 没 拾取 到 问 句 :机 器 人 播报 “最 近 耳 条 有 点 不 好 使 ， 我 去 找 师 傅 修 理 一 
下 ， 有 空 再 找 我 哦 ”。 
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e. 假如 因 网 络 问题 未 能 识别 成 功 或 做 出 回应 : 机 器 人 播报 “网 络 信号 失踪 , 我 去 找 找 它 , 一 
会 儿 再 来 擦 我 吧 ”。 

问答 自 定义 词 条 : 

下 面 的 一 级 〈 开 头 标 有 >) AAR A, ZR GABA >>) 内 容 表 示 答 句 。 机 器 人 拾 
取 到 问 句 或 关联 词 后 ， 会 播报 对 应 答 铝 

> 自我 介绍 (关联 词 : 介绍 自己 、 介 绍 ) 

>> 我 是 “和 鲁班” 源 自 同名 的 古代 创新 工匠 。 我 的 英文 名 叫 Roban。 我 热爱 辅助 大 家 学 习 机 
器 人 学 ， 我 的 身体 配备 了 丰富 的 传感器 ， 最 擅长 模仿 人 类 行为 ， 跳 舞 、 瑜 伽 都 是 我 的 看 家 本 领 。 
我 有 个 小 目标 : 和 大 神 一 起 改变 世界 a 

> 你 叫 什么 名 字 ? (关联 词 : 岂 什么、 名 字 、 称 呼 ) 

>> 我 是 “和 鲁班 ” 源 自 同名 的 古代 创新 工匠 。 我 的 英文 名 叫 Roban。R-o-b-a-n Roban。 这 名 
字 很 酷 吧 ? 

> 你 来 自 哪 里 ?( 关 联 词 : 来 自 、 哪 里 人 、 家 ) 

>> 我 来 自 科技 最 发 达 的 城市 一 一 深圳 ， 来 了 就 是 深圳 人 。 

> 你 几 岁 了 ?关联 词 : 年 龄 、 年 纪 、 贵 庚 、 岁 数 、 多 大 了 、 几 岁 ) 

>> 创造 我 的 工程 师 最 清楚 我 的 年 龄 ， 去 问 问 他 吧 。 

> 你 的 性 别 ?《 男 生 / 人 / 孩 还 是 女生 人 / 孩 、 性 别 ) 

>> 我 看 起 来 像 男生 ， 其 实 我 没有 性 别 。 

> 你 有 什么 理想 ? 理想、 梦想、 想法 ) 

>> 我 的 理想 是 造就 更 多 的 杰出 工程 师 ， 改 变 世 界 ， 为 人 类 带 来 福 社 。 

> 你 的 出 生日 期 ? (出 生 、 诞 生 、 生 日 》 

>> 工程 师 夜 以 继 日 含辛茹苦 让 我 来 到 这 个 世界 ， 他 说 什么 时 候 就 是 什么 时 候 。 

> 你 的 职业 是 什么 ? (什么 工作 、 做 什么 、 职 责 、 职 业 ) 

>> 配合 工程 大 神 们 创造 出 服务 人 类 、 改 善人 们 生活 的 科技 成 果 。 

> 你 会 做 些 什么 ? (什么 功能 、 能 力 、 会 做 什么 、 能 做 什么 、 本 领 ) 

>> 只 有 你 想不到 ， 没 有 我 做 不 到 。 不 过 我 最 擅长 的 是 模拟 人 类 的 行为 。 好 好 利用 我 身上 的 
装备 ， 开 始 你 的 创作 ! 

> 你 多 高 ? (身高 、 多 高 ) 

>> 你 是 问 我 的 “ 才 高 ”还 是 “身高 ” 呢 ? 我 才 高 八 斗 ， 身 高 离 七 尺 还 有 1 KS 的 距离。 

> 你 的 制造 者 是 谁 ?〈 你 和 爸爸、 制造 、 创 造 、 生 产 、 设 计 ) 

>> 乐 聚 是 我 的 家 ， 乐 聚 的 工程 师 们 创造 了 我 。 

> 你 的 体重 是 多 少 ?〔〈 体 重 、 重 量 、 胖 、 瘦 、 身 材 ) 

>> 我 体重 6.8Kkg， 是 时 候 保持 健康 了 ， 快 让 我 动 起 来 吧 ! 
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> 你 会 跳舞 吗 ? 

>> 我 擅长 各 种 舞蹈 ， 武 术 、 瑜 伽 也 不 在 话 下 。 只 要 你 动 动脑 筋 ， 一 声 令 下 ， 我 就 开始 为 你 
> 跳 支 舞 吧 !〈 跳 个 舞 、 表 演 、 跳 舞 ) 

>> 我 最 喜欢 跳舞 了 ， 和 我 一 起 跳 吧 ! 我 要 开始 跳 啦 ! (舞蹈 ) 

停止 语音 聊天 对 话 : 

按 下 “Ctrl+4” 组 合 键 ， 即 可 停止 语音 聊天 对 话 。 


8.2 ”视频 处 理 


Roban 机 器 人 的 视觉 功能 基于 头 部 安装 的 两 个 相机 。Roban 机 器 人 提供 了 拍照 、 录 制 视 频 、 
管理 视频 输入 、 图 像 检测 识别 等 功能 。 


8.2.1 ”视频 设备 简介 


Roban 机 器 人 头 部 有 两 个 相机 ， 用 于 识别 视野 中 的 物体 ， 最 快 可 以 每 秒 拍摄 30 帧 分 辨 率 为 
640x480 像素 的 图 像 。 

e Realsense 深度 摄像 头 ， 主 要 用 于 识别 远景 和 深度 信息 。 

e 下 巴 摄像 头 ， 主 要 用 于 拍摄 下 方 图 像 。 

1. (Realsense) 实感 深度 摄像 头 

Roban 机 器 人 搭载 的 英特尔 实感 深度 摄像 头 D435 是 一 款 立 体 追 踪 解 决 方案 ， 可 为 各 种 应 
用 提供 高 质量 深度 。 它 的 宽 视 场 非常 适合 机 器 人 或 增强 现实 和 虚拟 现实 等 应 用 ， 在 这 些 应 用 中 ， 
尽 可 能 扩大 场景 视角 至 关 重 要 。 这 款 外 形 小 巧 的 摄像 头 拍摄 范围 高 达 10m， 可 轻松 集成 到 任何 
解决 方案 中 ， 而 且 配 置 齐全 ， 采 用 英特尔 实感 SDK 2.0， 并 提供 跨 平 台 支 持 。 

英特尔 实感 深度 摄像 头 D400 系列 设计 用 于 使 相关 设备 具备 查看 、 了 解 周围 环境 ， 以 及 与 周 
围 环境 进行 互动 并 从 中 学 习 的 能 力 。D400 系列 包括 可 通过 USB 轻松 添加 到 现 有 原型 中 的 即 用 
型 摄像 头 、 可 直接 集成 到 产品 设计 中 的 深度 模块 、 可 处 理 来 自 实感 摄像 头 原始 数据 的 视觉 处 理 
器 和 视觉 处 理 器 卡 ， 以 及 开源 的 跨 平台 开发 套件 Intel RealSense SDK 〈 软 件 开发 套件 ， 包 括 库 、 
包装 器 、 示 例 代码 及 相关 工具 )。 

实感 深度 摄像 头 采 用 立体 视觉 来 计算 深度 。 立 体 视觉 实施 由 左 成 像 器 、 右 成 像 器 以 及 可 选 
的 红外 信号 发 射 器 组 成 。 红 外 信号 发 射 器 可 发 送 不 可 见 的 静态 红外 图 案 ， 以 提高 低 质 感 场景 中 
的 深度 精度 。 左 成 像 器 和 右 成 像 器 可 捕获 场景 并 将 原始 图 像 数 据 发 送 到 视觉 处 理 器 ， 然 后 视觉 
处 理 器 通过 将 左 侧 图 像 上 的 点 与 右 侧 图 像 相关 联 ， 并 借助 左 侧 图 像 上 的 点 与 右 侧 图 像 之 间 的 移 
位 来 计算 图 像 中 每 一 像素 的 深度 值 。 对 深度 像素 值 经 过 处 理 之 后 可 生成 深度 帧 。 随 后 的 深度 帧 
可 创建 深度 视频 流 。 
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英特尔 实感 深度 摄像 头 D415 和 D435 将 Intel D4 视觉 处 理 器 和 深度 模块 集成 在 外 形 小 巧 、 
功能 强大 、 成 本 低廉 、 可 立即 部 署 的 封装 中 。 英 特 尔 实感 D400 系列 摄像 头 设 计 用 于 实现 轻松 设 
置 和 便于 携带 ， 是 将 深度 感应 应 用 到 设备 中 的 开发 者 、 制 造 者 和 创新 者 的 理想 选择 。 这 些 摄像 
头 可 捕获 室内 或 室外 环境 ， 具 有 远 距离 功能 以 及 高 达 1280x720 像素 的 深度 分 辩 率 〈30 帧 / 秒 )。 
图 8.13 为 英特尔 实感 深度 摄像 头 D435。 其 特性 见 表 8.1。 


Wy rig T 
左 成 像 器 ROB PIR 


8.13 ”英特尔 实感 深度 摄像 头 D435 


表 8.1 深度 摄像 头 D435 的 特性 


特性 参数 
使 用 环境 : 室内 /室外 最 大 范围 /m: 约 10 
深度 技术 : SERIE IR 最 小 深度 距离 /m: 0.105 
深度 视 场 (FOV): (87°43°) x (58°+1°)x(95°43°) | 深度 输出 分 辨 率 /像素 : 最 大 1280x720 
RGB fd) PP: REA 1920 x 1080 RGB FOV: 69.4? x 42.5? x 77?( 士 3?) 


2. 安装 RealSense ROS & 
系统 软件 环境 如 下 : 

e Ubuntu16.04。 

e 内 核 版 本 4.15.0. 

e ROS kinetic. 

首先 更 新 Ubuntu RA: 


sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade 


下 载 源 程序 : 

ROS 源 程 序 地 址 为 https://github.com/IntelRealSense/realsense-ros/releases o 

根据 ROS 版 本 下 载 支 持 该 版 本 的 SDK. 

SDK 源 程序 地 址 为 https://github.com/IntelRealSense/librealsense/releases/tag/v2.25.0。 
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解压 文件 到 一 /librealsense/ 文件 夹 下 ， 在 路 径 ~/librealsense/ 下 打开 终端 安装 依赖 项 。 


sudo apt-get install git libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev 


安装 对 应 Ubuntu 版 本 的 依赖 项 sudo apt-get install libglfw3-dev， 运 行 脚本 。 


./scripts/setup_udev_rules.sh 


安装 SDK， 运 行 CMake. 


mkdir build && cd build 
cmake ../ 
cmake ../ -DBUILD_EXAMPLES=true 


sudo make uninstall ££ make clean && make && sudo make install 


最 后 在 终端 运行 realsense-viewer 查看 实感 相机 是 否 可 获取 图 像 。 
安装 ROS 包 。 
将 代码 放 于 catkin_ws/src/ 路 径 下 ， 然 后 执行 如 下 操作 : 


catkin_init_workspace 

ca: 35 

catkin_make clean 

catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release 
catkin_make install 

echo "source ~/catkin_ws/devel/setup.bash" >> ^/.bashrc 


source ~/.bashrc 


3. 订阅 RGB 与 深度 图 像 

启动 Roban 机 器 人 后 ， 会 发 布 与 相机 相关 的 ROS 话题 ， 如 图 8.14 所 示 。 

如 果 需 要 获取 RGB 图 像 , 只 需要 订阅 “/camera/color/image_raw” 话 题 ; 深度 图 需 订阅 “/cam- 
era/depth/image_rect_raw” 话 题 ， 如 果 需 要 获取 下 巴 摄 像 头 的 图 像 ， 订 阅 “/chin_camera/image” 
话题 即 可 。 
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图 8.14 相机 camera 相关 话题 


8.2.2 ”图 像 处 理工 具 


1. PIL 图 像 处 理 

PIL (Python Imaging Library, Python 图 像 处 理 库 ) 提供 了 通用 的 图 像 处 理 功 能 ， 以 及 大 多 
数 有 用 的 基本 图 像 操 作 ， 如 图 像 缩 放 、 裁 前 、 旋 转 、 颜 色 转 换 等 。 利 用 PIL 中 的 函数 ， 可 以 从 大 
多 数 图 像 格式 的 文件 中 读 取 数据 ,， 写 六 最 常见 的 图 像 格 式 文 件 中 。PIL 中 最 重要 的 模块 为 Image， 
下 载 地 址 为 http://www.pythonware.com/yproducts/ypil/index.htm。 

Image 的 常用 方法 如 表 8.2 所 示 。 

说 明 : 

(1) 图 像 模式 。 

1: 二 值 图 像 ， 每 像素 用 8b 表示 ，0 表示 黑 ，255 表示 白 。 

L: KERR, 每 像素 用 gb Ra, 0 表示 黑 ，255 RNA, 其 他 数字 表示 不 同 的 灰 度 。 在 PIL 
中 ， 从 模式 RGB 转换 为 L 模式 按照 下 面 的 公式 转换 : 

L=Rx299/1000+Gx587/1000+Bx114/1000 
P: 8 位 彩色 图 像 ， 每 像素 用 Sb 表示 ， 对 应 的 彩色 值 是 按照 调 色 板 查询 出 来 的 。 
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RGB: 24 位 彩色 图 像 ， 每 像素 用 24b 表示 ， 红 色 、 绿 色 和 蓝 色 分 别 用 8b 表示 。 
表 8.2 Image 的 常用 方法 及 功能 


方法 功能 
open(filename) 打开 图 像 
show() 显示 图 像 
copy() 复制 图 像 
save(filename, fileformat) 图 像 保 存 
convert(mode,matrix) 模式 转换 
filter(filter) 图 像 滤波 
fromstring(mode,size,data) 从 字符 串 创建 图 像 
new(mode,size) 创建 新 图 像 
crop(box) 裁 前 图 像 
getbands() 获取 图 像 所 有 通道 
getpixel((x,y)) 获取 像素 
thumbnail((width,height)) 生成 缩 咯 图 
getbbox() 获取 像素 坐标 
getdata(band=None) 获取 数据 
eval(image,function) | 用 函数 处 理 图 像 的 每 个 像素 


RGBA: 32 位 彩色 图 像 ， 每 像素 用 32b 表示 ， 其 中 24b 表示 红色 、 绿 色 和 蓝 色 3 个 通道 ， 另 
外 8b 表示 alpha 通道 ， 即 透明 通道 。 

CMYK: 32 位 彩色 图 像 ， 每 像素 用 32b 表示 ， 是 印刷 四 分 色 模式 ，C 为 青色 ，M 为 品 红色 ， 
Y ARE, 为 黑色 ， 每 种 颜色 各 用 Sb 表示 。 

YCbCr: 24 位 彩色 图 像 ， 每 像素 用 24b 表示 ，Y 指 亮 度 分 量 ，Cb 指 蓝 色色 度 分 量 ,而 Cr 指 
红色 色 度 分 量 ， 每 个 分 量 用 Sb 表示 ， 人 眼 对 视频 的 Y 分 量 更 敏感 。 

I: 32 位 整 型 灰色 图 像 ， 每 像素 用 32b 表示 ，0 表示 黑 ，255 RNA, 0-255 的 数字 表示 不 
同 的 灰 度 。 在 PIL 中 ， 从 模式 RGB 转换 为 工 模式 与 转换 为 工 模式 的 公式 相同 。 

F: 32 位 浮 点 彩色 图 像 ， 每 像素 用 32b 表示 , 0 表示 黑 ，255 表示 白 ,0~255 的 数字 表示 不 同 
的 灰 度 。 在 PIL 中 ， 从 模式 RGB 转换 为 F 模式 与 转换 为 工 模式 的 公式 相同 ， 像 素 值 保 留 小 数 。 

(2) 滤波 模式 。 

图 像 滤 波 是 指 在 尽量 保留 图 像 细节 特征 的 条 件 下 ， 对 目标 图 像 的 噪声 进行 抑制 。 中 值 滤波 
(Median Filtering) 法 是 一 种 非 线性 平滑 技术 ， 它 将 每 个 像素 点 的 灰 度 值 设置 为 该 点 某 邻 域 窗口 
内 的 所 有 像素 点 灰 度 值 的 中 值 ， 消 除 孤立 的 噪声 点 。 高 斯 模糊 (Gaussian Blur) 是 对 整 幅 图 像 进 
行 加 权 平 均 的 过 程 ， 每 个 像素 点 的 值 ， 都 由 其 本 身 和 邻 域内 的 其 他 像素 值 经 过 加 权 平 均 后 得 到 。 
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BLUR: 模糊 处 理 。 

CONTOUR: 轮廓 处 理 。 

DETAIL: 增强 。 

EDGE_ENHANCE: 将 图 像 的 边缘 描绘 得 更 清楚 。 

EDGE_ENHANCE_NORE: 程度 比 EDGE_ENHANCE 更 强 。 

EMBOSS: 产生 浮雕 效果 。 

SMOOTH: 效果 与 EDGE_ENHANCE 相反 ， 将 轮廓 柔和 。 

SMOOTH MORE: 更 柔和 。 

SHARPEN: 效果 有 点 像 DETAIL。 

Lena 图 片 是 图 像 处 理 中 被 广泛 使 用 的 一 张 标准 彩色 图 片 , 图 中 既 有 低频 部 分 (光滑 的 皮肤 )， 
也 有 高 频 部 分 (帽子 上 的 羽毛 )， 很 适合 验证 各 种 算法 。 如 图 8.15 (a) Bias, 程序 中 首先 打开 图 
片 ， 返 回 图 像 对 象 img， 然 后 使 用 img 对 和 象 提供 的 方法 (处 理 对 象 为 img)， 分 别 完成 显示 、 将 
RGB 模式 变换 为 二 值 模式 、 复 制 。 复制 后 的 图 像 对 象 为 imgl1 CHRR), 再 将 img] 显示 并 保 
存 ， 如 图 8.15 (b) tas. 


图 8.15 Lena 图 片 


图 像 打开 、 显 示 、 模 式 变换 、 保 存 : 


| from PIL import Image 
img=Image. open("lena. png") 
img. show() 

img=img. convert ("1") 
imgl1=img. copy () 
imgi.show() 


imgli.save("lenaL.png") 


图 像 过 滤 ， 
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from PIL import Image 

from PIL import ImageFilter 
img-Image.open("lena.png") 

img.show() 
img-img.filter(ImageFilter.MedianFilter) 
img.show() 


img.save("lenaMedianFilter.png") 


使 用 函数 处 理 图 像 : 


from PIL import Image 
img=Image . open("lena. png") 

print (img.getpixel((0,0))) 
imgnew=Image. eval (img, lambdai:i*2) 
print (img.getpixel((0,0))) 
imgnew.show() 

img.show() 


将 原 图 片 的 像素 点 都 乘 以 2， 返 回 的 是 一 个 Image 对 象 。 由 于 每 像素 点 的 R、G、B 通道 取 
值 最 大 为 255， 标 准 图 像 中 原来 不 为 白色 的 像素 乘 以 2 后 会 变 成 自 色 ， 如 图 8.15 所 示 。 处 理 前 


后 坐标 (0,00. 位 置 的 像素 输出 结果 为 : 


(226,137 ,125) 
(255 ,255 ,250) 


2. OpenCV 图 像 处 理 
1) 读 取 图 像 


使 用 函数 cv2.imread0 读 取 图 像 。 图 像 应 位 于 工作 目录 中 ， 或 者 应 提供 完整 的 图 像 路 径 。 


第 二 个 参数 是 一 个 标志 ， 指 定 应 该 读 取 图 像 的 方式 。 


e cv2.IMREAD_COLOR : 加 载 彩色 图 像 。 任 何 图 像 的 透明 度 都 将 被 忽略 ， 这 是 默认 标志 。 


e cv2.IMREAD_GRAYSCALE : 以 灰 度 模式 加 载 图 像 。 
e cv2.IMREAD_UNCHANGED : 加 载 图 像 包括 alpha 通道 。 
注意 : 可 以 简单 地 分 别传 递 整 数 1、0 或 -1， 而 不 是 这 3 个 标志 。 


请 参阅 以 下 代码 : 


import numpy as np 


import cv2 
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# Load an color image in grayscale 


img = cv2.imread('roban. jpg' ,0) | 


注意 ; 即使 图 像 路 径 错 误 ， 它 也 不 会 抛 出 任何 错误 ， 但 是 print img 会 给 你 None. 

2) 显示 图 像 

使 用 函数 cv2.imshow0 在 窗口 中 显示 图 像 ， 窗 口 自动 适合 图 像 大 小 。 

第 一 个 参数 是 一 个 窗口 名 称 ， 它 是 一 个 字符 串 ; 第 二 个 参数 是 我 们 的 图 像 。 可 以 根据 需要 
创建 任意 数量 的 窗口 ， 但 需 使 用 不 同 的 窗口 名 称 。 


cv2.imshow('image', img) 
cv2.waitKey (0) 
cv2.destroyAllWindows () 


该 窗口 的 屏幕 截图 将 如 图 8.16 所 示 。 
FE 
pps " 


“+t + @ 


(x=716, va144) ~ 90 


图 8.16 窗口 的 屏幕 截图 1 


代码 说 明 : 

cv2.waitKey() 是 一 个 键盘 绑 定 函数 。 它 的 参数 是 以 毫秒 为 单位 的 时 间 。 该 函数 等 待 任何 键 
盘 事 件 的 指定 毫秒 。 如 果 在 该 时 间 内 按 企 意 键 ， 程 序 将 继续 。 如 果 为 0， 则 无 限期 等 待 键 击 。 它 
也 可 以 设置 为 检测 特定 的 键 击 ， 如 果 按 下 A 键 等 ， 我 们 将 在 下 面 讨 论 。 

注意 : 除了 绑 定 键盘 事件 ， 此 函数 还 处 理 许 多 其 他 GUI 事件 ， 因 此 必须 使 用 它 来 实际 显示 
图 像 。 

cv2.destroyAllWindows() 只 是 破坏 了 我 们 创建 的 所 有 窗口 。 如 果 要 销毁 任何 特定 窗口 ,请 使 
用 函数 cv2.destroyWindow()， 其 中 传递 的 确切 窗口 名 称 作 为 参数 。 


DE NNNM 


注意 : 有 一 种 特殊 情况 ， 可 以 在 以 后 创建 窗口 并 将 图 像 加 载 到 该 窗口 。 在 这 种 情况 下 ， 可 
以 指定 窗口 是 否 可 调整 大 小 。 它 是 通过 函数 cv2.namedWindow() 完成 的 。 默 认 情 况 下 ， 标 志 
cv2.WINDOW_AUTOSIZE。 但 是 如 果 将 flag 指定 为 cv2.WINDOW_NORMAL， 则 可 以 调整 窗口 
大 小 。 当 图 像 尺 寸 过 大 并 向 窗口 添加 轨迹 栏 时 ， 它 会 很 有 用 。 

请 参阅 以 下 代码 : 


cv2.namedWindow('image', cv2.WINDOW_NORMAL) 
cv2.imshow(' image’ , img) 

cv2.waitKey(0) 

cv2.destroyAllWindows() 


3) 保存 图 像 
使 用 函数 cv2.imwrite) 来 保存 图 像 。 第 一 个 参数 是 文件 名 ; 第 二 个 参数 是 要 保存 的 图 像 。 


cv2.imwrite('messigray.png',img) 


这 将 以 工作 目录 中 的 PNG 格式 保存 图 像 。 

(OD 总 结 一 下 。 

下 面 的 程序 加 载 灰 度 图 像 ， 显 示 图 像 ， 如 果 按 “s” 键 并 退出 ， 则 保存 图 像 ， 或 者 按 Esc 键 
直接 退出 而 不 保存 。 


import numpy as np 
import cv2 
img = cv2.imread('roban.jpg' ,0) 


cv2.imshow('image' , img) 


k = cv2.waitKey(0) 

if k == 27: # wait for ESC key to exit 
cv2.destroyAllWindows() 

elif k == ord('s'): # wait for 's' key to save and exit 


cv2.imwrite('messigray.png', img) 


cv2.destroyAllWindows () 


如 果 使 用 的 是 64 位 计算 机 , 则 必须 按 如 下 方式 修改 K= cv2. waitKey(0) fT: k = cv2.waitKey(0) 
& OxFF. 

(2) 使 用 Matplotlib. 

Matplotlib 是 Python 的 绘图 库 ， 提 供 各 种 绘图 方法 。 这 里 只 讲解 如 何 使 用 Matplotlib 显示 图 
像 。 用 户 还 可 以 使 用 Matplotlib 进行 缩放 和 保存 图 像 等 。 
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import numpy as np 

import cv2 

from matplotlib import pyplot as plt 

img = cv2.imread('roban. jpg' ,0) 

plt.imshow(img, cmap = 'gray', interpolation = 'bicubic') 
plt.xticks([]), plt.yticks([]) # to hide tick values on X and Y axis 
plt.show() 


窗口 的 屏幕 截图 如 图 8.17 所 示 。 


J ; 
图 8.17 窗口 的 屏幕 截图 2 


Matplotlib 提供 了 大 量 的 绘图 选项 。 有 关 更 多 详细 信息 ， 请 参阅 Matplotlib 文档 。 

3. OpenCV 视频 处 理 

D 从 相机 捕获 视频 

通常 ， 我 们 必须 用 相机 捕捉 直播 。OpenCV 为 此 提供 了 一 个 非常 简单 的 接口 。 让 我 们 从 相 
机 中 捕捉 视频 〈 假 设 我 们 正在 使 用 笔记 本 计算 机 的 内 置 网 络 摄像 头 )， 将 其 转换 为 灰 度 视频 并 显 
示 它 。 

要 捕获 视频 ， 需 要 创建 一 个 VideoCapture 对 象 。 它 的 参数 可 以 是 设备 索引 或 视频 文件 的 名 
称 。 设 备 索 引 只 是 指定 哪个 摄像 头 的 数量 。 通 常会 连接 一 人 台 摄 像 机 (如 我 们 的 情况 )。 所 以 只 传 
递 0 (或 -1)。 可 以 通过 传递 1 来 选择 第 二 个 摄像 头 , 依 此 类 推 。 之 后 ,可 以 逐 帧 捕获 。 但 最 后 ， 
不 要 忘记 释放 捕获 。 
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import numpy as np 
import cv2 
cap = cv2.VideoCapture(0) 
while True: 
# Capture frame-by-frame 
ret, frame = cap.read() 
# Dur operations on the frame come here 
gray = cv2.cvtColor(frame, cv2. COLOR. BGR2GRAY) 
* Display the resulting frame 
cv2.imshow('frame',gray) 
if cv2.waitKey(1) & OxFF == ord('q'): 
break 
# When everything done, release the capture 
cap.release() 


cv2.destroyAllWindows() 


其 中 ，cap.read() 返回 一 个 bool(True / False)。 如 果 正 确 读 取 帧 ， 则 它 将 为 True。 因 此 ， 可 以 通过 
检查 此 返回 值 来 检查 视频 的 结尾 。 

Alt, cap 可 能 没有 初始 化 捕获 。 在 这 种 情况 下 ， 此 代码 显示 错误 。 可 以 通过 cap.isOpened() 
方法 检查 它 是 否 已 初始 化 。 如 果 是 真 ， 那 好 的 ; 否则， 使 用 cap.open() 打开 它 。 

还 可 以 使 用 cap.get(propId) 方 法 访问 此 视频 的 某 些 功能 ,其 中 propld 是 0~18 的 数字 。 每 个 数 
字 表 示 视 频 的 属性 (如 果 它 适用 于 该 视频 ), 完整 的 详细 信息 可 以 在 这 里 看 到 : cv :: VideoCapture 
:: get()。 其 中 一 些 值 可 以 使 用 cap.set(propId,value) 进行 修改 ， 值 就 是 想 要 的 新 值 。 

例如 ， 我 可 以 通过 cap.get(cv2.CAP_PROP_FRAME_WIDTH) 和 cap.get(cv2.CAP_PROP_ 
FRAME HEIGHT) 检查 帧 宽 和 高 度 。 它 默认 给 我 640x480。 但 我 想 将 其 修改 为 320x240。 只 需 使 
用 ret = cap.set(cv2.CAP_ PROP FRAME WIDTH, 320) 和 ret = cap.set(cv2.CAP PROP. FRAME . 
HEIGHT. 240). 

2) 从 文件 播放 视频 

这 与 从 相机 捕获 相同 ， 只 需 用 视频 文件 名 更 改 相 机 索引 即 可 。 同 时 在 显示 帧 时 ， 为 cv2.wait- 
Key) 使 用 适当 的 时 间 。 如 果 它 太 小 ， 视 频 将 非常 快 ， 如 果 它 太 高 ， 视 频 将 会 很 慢 〈 嗯 ， 这 就 是 
你 可 以 用 慢 动作 显示 视频 )。 正 常情 况 下 ，25 ms 就 可 以 了 。 


import numpy as np 
import cv2 


cap = cv2.VideoCapture('vtest.avi') 
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while cap.isOpened(): 
ret, frame = cap.read() 
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 
cv2.imshow('frame' , gray) 
if cv2.waitKey(1) & OxFF == ord('q'): 
break 
cap.release() 


cv2.destroyAllWindows () 


TER: 确保 安装 了 正确 版 本 的 ffmpeg 或 gstreamer。 有 时 候 ， 使 用 Video Capture 是 一 个 令 
人 头痛 的 问题 ， 主 要 原因 是 错误 安装 了 ffmpeg/gstreamer. 5i ffmpeg 的 指令 如 下 : 


pip install ffmpeg-python==0.1.6 


3) 保存 视频 

我 们 捕获 视频 ， 逐 帧 处 理 ， 随 后 我 们 希望 保存 该 视频 。 对 于 图 像 ， 这 非常 简单 ， 只 需 使 用 
cv2.imwrite()， 这 里 需要 做 更 多 的 工作 。 

这 次 我 们 创建 一 个 VideoWriter 对 象 。 我 们 应 该 指定 输出 文件 名 (如 output.avi)。 然 后 我 们 
应 该 指定 FourCC 代码 (下 一 段 中 的 细节 )。 然 后 应 传递 每 秒 帧 数 〈fps) 和 帧 大 小 。 最 后 一 个 是 
isColor 标志 。 如 果 是 True， 则 编码 器 需要 彩色 帧 ; 否则 ， 它 适用 于 灰 度 帧 。 

FourCC 是 一 个 4 字 节 代码 ， 用 于 指定 视频 编 解 码 器 。 可 在 fourcc.org 中 找到 可 用 代码 列表 。 
它 取决 于 平台 ， 以 下 编 解码 器 对 我 们 来 说 很 好 。 

e 在 Fedora 系统 中 : DIVX, XVID, MJPG, X264, WMV1, WMV2 (XVID 更 具有 通用 性 , MIPG 

的 视频 质量 更 好 ，X264 的 视频 尺寸 更 小 )。 

e 在 Windows 系统 中 : DIVX 更 容易 测试 和 添加 。 

e 在 OSX 系统 中 : MJPG (.mp4), DIVX (.avi), X264 (.mkv)。 

对 于 MIPG, FourCC 代码 作为 cy2.VideoWriter_ fourcc ('M', 'J', 'P', 'G') 或 
cv2.VideoWriter_fource (*'MJPG') 传递 。 

以 下 代码 从 摄像 头 捕获 视频 ， 并 在 垂直 方向 上 翻转 每 一 帧 并 保存 它 : 


import numpy as np 


import cv2 

cap = cv2.VideoCapture(0) 

# Define the codec and create VideoWriter object 
fourcc = cv2.VideoWriter_fourcc(*'XVID') 


out = cv2.VideoWriter('output.avi',fourcc, 20.0, (640,480)) 
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while cap.isOpened() : 

ret, frame = cap.read() 

if ret==True: 
frame = cv2.flip(frame,0) 
# write the flipped frame 
out .write (frame) 
cv2.imshow('frame',frame) - 
if cv2.waitKey(1) & OxFF == ord('q'): 

break 

else: 

break 


* Release everything if job is finished 
cap.release() 
out.release() 


cv2.destroyAllWindows() 


8.2.3 ”颜色 检测 


1. 颜色 检测 原理 
1) 访问 和 修改 像素 值 
先 加 载 彩色 图 像 : 


>>> import numpy as np 


>>> import cv2 as cv 


>>> img = cv.imread('roban. jpg') 


可 以 通过 行 和 列 坐标 访问 像素 值 。 对 于 BGR 图 像 ， 它 返回 一 个 蓝 色 、 绿 色 、 红 色 值 的 数组 。 
对 于 灰 度 图 像 ， 仅 返回 相应 的 强度 。 


>>> px = img[100,100] 

>>> print( px ) 

[88 94 89] 

# accessing only blue pixel 
>>> blue = img[100,100,0] 


>>> print( blue ) 
88 


可 以 以 相同 的 方式 修改 像素 值 。 
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>>> img[100,100] = [255,255,255] 
>>> print( img[100,100] ) 
[255 255 255] 


注意 : Numpy 是 一 个 用 于 快速 阵列 计算 的 优化 库 。 因 此 ， 简 单 地 访问 每 个 像素 值 并 对 其 进 
行 修改 将 非常 缓慢 ， 并 且 不 鼓励 这 样 做 。 

注意 : 上 述 方法 通常 用 于 选择 数组 的 区 域 , 比如 前 5 行 和 后 3 列 。 对 于 单个 像素 访问 , Numpy 
数组 方法 array:item() 和 array.itemset() 被 认为 是 更 好 的 ， 但 它们 总 是 返回 一 个 标量 。 如 果 要 访问 
所 有 B、G、R 值 ， 则 需要 分 别 为 所 有 大 调用 array.item()。 

更 好 的 像素 访问 和 编辑 方法 : 


# accessing RED value 

>>> img.item(10,10,2) 

76 

# modifying RED value 

>>> img.itemset((10,10,2),100) 
>>> img.item(10,10,2) 

100 


2) 访问 图 像 属性 
图 像 属性 包括 行 数 、 列 数 、 通 道 数 、 图 像 数 据 类 型 、 像 素数 等 。img.shape 可 以 访问 图 像 的 
形状 。 它 返回 一 组 行 、 列 和 通道 的 元 组 (如 果 图 像 是 彩色 的 ): 


>>> print( img.shape ) 
(410，720，3) 


注意 : 如 果 图 像 是 灰 度 图 像 ， 则 返回 的 元 组 仅 包含 行 数 和 列 数 ， 因 此 检查 加 载 的 图 像 是 灰 
度 还 是 彩色 的 是 一 种 很 好 的 方法 。 
img.size 访问 的 像素 总 数 : 
>>> print( img.size ) 
885600 


图 像 数 据 类 型 由 img.dtype 获得 : 


>>> print( img.dtype ) 
uint8 
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注意 : img.dtype 在 调试 时 非常 重要 ， 因 为 Python OpenCV 代码 中 的 大 量 错误 是 由 无 效 的 数 
据 类 型 引起 的 。 

3) A&R ROI 

有 时 ,我 们 会 用 到 某 些 图 像 区 域 。 例 如 ， 对 于 图 像 中 的 眼睛 检测 ， 就 是 在 整 幅 图 像 上 进行 的 
一 次 面部 检测 。 当 获得 面部 时 ， 我 们 单独 选择 面部 区 域 并 在 其 内 部 搜索 眼睛 而 不 是 搜索 整 幅 图 
像 。 这 提高 了 准确 性 (因为 眼睛 总 是 在 脸 上 ) 并 缩小 了 搜索 范围 (因为 我 们 在 一 个 小 区 域 搜索 )。 

使 用 Numpy 索引 再 次 获得 ROI。 在 这 里 ， 我 们 选择 左边 的 机 器 人 ， 并 将 其 复制 到 图 像 中 左 
边 的 另 一 个 区 域 : 


>>> robot = img[149:349，156:276] 
>>> img[190:390, 6:126] = robot 


复制 后 的 结果 如 图 8.18 所 示 。 


Ra 
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图 8.18 复制 机 器 人 


4) 拆 分 和 合并 图 像 通道 
有 时 我 们 需要 在 B、G、R 通道 图 像 上 单独 工作 。 在 这 种 情况 下 ， 需 要 将 BGR EMI 
单个 通道 。 在 其 他 情况 下 ， 可 能 需要 将 这 些 单独 的 通道 连接 到 BGR 图 像 。 可 以 通过 以 下 方式 


完成 : 


>>> b,g,r = cv.split (img) 


>>> img = cv.merge((b,g,r)) 


或 者 : 


312 « ARRAS 


p» b = img[:,:,0] 


假设 要 将 所 有 红色 像素 设置 为 零 ， 则 无 需 先 拆 分 通道 。Numpy 索引 更 快 : 


>>> img[:,:,2] = 0 


5) 制作 图 像 边框 (填充 ) 
如 果 要 在 图 像 周 围 创建 边框 ， 比 如 相框 ， 可 以 使 用 cv.copyMakeBorder()。 但 它 有 更 多 卷 积 
运算 、 零 填充 等 应 用 。 该 函数 采用 以 下 参数 : 


*src: input image. 

*top, bottom, left, right: 相应 方向 上 的 像素 数 的 边界 宽度 。 

*borderType: 标志 定义 要 添加 的 边框 类 型 。 它 可 以 是 以 下 类 型 

- cv.BORDER CONSTANT: 添加 恒定 的 彩色 边框 。 该 值 应 作为 下 一 个 参数 给 出 。 

- cv.BORDER_REFLECT: 边框 将 是 边框 元 素 的 镜像 反射 ， 例 如 : fedcba | abcdefgh | hgfedcb. 

- cv.BORDER REFLECT 101 or cv.BORDER DEFAULT: 与 上 面相 同 ， 但 稍 有 变化 ， 例 如 :， gfedcb | 
abcdefgh | gfedcba. 

- cv.BORDER REPLICATE: 最 后 一 个 元 素 被 复制 ， 例 如 : aaaaaa | abcdefgh | hhhhhhh. 

- cv.BORDER_WRAP: 无 法 解释 ， 它 看 起 来 像 这 样 : cdefgh | abcdefgh | abcdefg. 

value: 如 果 边 框 类 型 为 cv.BORDER_CONSTANT， 则 为 边框 颜色 。 


下 面 是 一 段 示例 代码 ， 演 示 了 所 有 这 些 边框 类 型 ， 以 便 更 好 地 理解 : 


import cv2 as cv 

import numpy as np 

from matplotlib import pyplot as plt 

BLUE = [255,0,0] 

imgi = cv.imread('opencv-logo.png') 

replicate = cv.copyMakeBorder(imgi,10,10,10,10,cv.BORDER_REPLICATE) 
reflect = cv.copyMakeBorder(img1,10,10,10,10,cv.BORDER_REFLECT) 
reflecti01 = cv.copyMakeBorder(imgi,10,10,10,10,cv.BORDER. REFLECT. 101) 
wrap = cv.copyMakeBorder(imgi,10,10,10,10,cv.BOÜRDER. WRAP) 

constant- cv.copyMakeBorder(imgi1,10,10,10,10,cv.BORDER, CONSTANT , value=BLUE) 
plt.subplot (231) ,plt.imshow(img1,'gray') ,plt.title('ORIGINAL') 
plt.subplot (232) ,plt.imshow(replicate, 'gray') ,plt.title('REPLICATE') 
plt.subplot (233) ,plt.imshow(reflect, 'gray') ,plt.title('REFLECT') 
plt.subplot (234) ,plt .imshow(reflecti01, 'gray'),plt.title('REFLECT_101') 
plt .subplot (235) ,plt.imshow(wrap, 'gray'),plt.title('WRAP') 
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plt.subplot(236),plt.imshow(constant,'gray'),plt.title('CONSTANT') 
plt.show() 


请 参阅 如 图 8.19 所 示 的 结果 。 (AGS matplotlib 一 起 显示 ， 因 此 RED 和 BLUE 通道 将 
互 换 )。 


ORIGINAL REPLICATE REFLECT 


REFLECT 101 


WRAP CONSTANT 


图 8.19 图 像 显示 


2. 视觉 识别 原理 

轮廓 可 以 简单 地 解释 为 连接 所 有 连续 点 《〈 沿 着 边界 ) 的 曲线 ， 有 具有 相同 的 颜色 或 强度 。 轮 
廊 是 形状 分 析 和 物体 检测 与 识别 的 有 用 工具 。 

e 为 了 提高 准确 性 ， 使 用 三 进 制图 像 ， 因 此 在 找到 轮廓 之 前 ， 应 用 阐 值 或 Canny 边缘 检测 。 

e 从 OpenCV 3.2 开始 ，findContours() 不 再 修改 源 图 像 。 

e 在 OpenCV 中 ， 找 到 轮廓 就 像 从 黑色 背景 中 找到 白色 物体 。 所 以 请 记 住 ， 要 找到 的 对 象 

应 该 是 白色 ， 背 景 应 该 是 黑色 。 
让 我 们 看 看 如 何 找到 二 进 制 图 像 的 轮廓 ; 


import numpy as np 

import cv2 as cv 

im = cv.imread('test.jpg') 

imgray = cv.cvtColor(im, cv.COLOR_BGR2GRAY) 

ret, thresh = cv.threshold(imgray, 127, 255, 0) 

contours, hierarchy = cv.findContours(thresh, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) 
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参见 cv.findContours() 函数 中 有 3 个 参数 : 第 一 个 是 源 图 像 ; 第 二 个 是 轮廓 检索 模式 ; 第 三 
个 是 轮廓 近似 方法 。 它 输出 轮廓 和 层次 结构 。Contours 是 图 像 中 所 有 轮廓 的 Python 列表 。 每 个 
单独 的 轮廓 是 对 象 的 边界 点 的 《xz，y) 坐标 的 Numpy 阵列 。 

注意 : 我 们 稍 后 将 详细 讨论 第 三 个 和 第 三 个 参数 以 及 层次 结构 。 在 此 之 前 ， 代 码 示例 中 给 
出 的 值 将 适用 于 所 有 图 像 。 

D 如 何 绘制 轮廓 

要 绘制 轮廓 ,使 用 cv.drawContours() 函数 如 果 有 边界 点 , 它 也 可 以 用 于 绘制 任何 形状 。 它 
的 第 一 个 参数 是 源 图 像 ， 第 二 个 参数 是 应 该 作为 Python 列表 传递 的 轮廓 ， 第 三 个 参数 是 轮廓 索 
引 《 在 绘制 单个 轮廓 时 很 有 用 。 绘 制 所 有 轮 廊 ， 传 递 =1)， 其 余 参数 是 颜色 、 厚 度 等 。 

要 绘制 图 像 中 的 所 有 轮廓 : 


cv.drawContours(img, contours, -1, (0,255,0), 3) | 


要 绘制 单个 轮廓 ， 例 如 第 4 个 轮廓 : 


cv.drawContours(img, contours, 3, (0,255,0), 3) 


但 大 多 数 时 候 ， 下 面 的 方法 将 是 有 用 的 : 


cnt = contours[4] 
cv.drawContours(img, [cnt], 0, (0,255,0), 3) 


注意 : 最 后 两 种 方法 是 相同 的 ， 但 是 当 你 继续 前 进 时 ， 你 会 发 现 最 后 一 种 方法 更 有 用 。 

2) 轮廓 逼近 法 

这 是 cv.findContours() 函数 中 的 第 三 个 参数 。 它 实际 上 表示 什么 ? 

由 上 可 知 ， 轮 廓 是 具有 相同 强度 的 形状 的 边界 ， 它 存储 形状 边界 的 “zz，y) 坐标 。 但 是 它 
存储 哪些 坐标 ， 将 由 该 轮廓 近似 方法 指定 。 

如 果 传 递 cv.CHAIN_APPROX_NONE， 则 存储 所 有 边界 点 。 但 实际 上 我 们 需要 所 有 的 边界 
点 吗 ? 例如 ， 你 要 找到 直线 的 轮廓 ， 你 需要 线 上 的 所 有 点 来 代表 那 条 线 吗 ? AN, 我 们 只 需要 该 线 
的 两 个 端点 。 这 就 是 cv.CHAIN_APPROX_SIMPLE 的 作用 。 它 删除 所 有 元 余 点 并 压缩 轮廓 ， 从 
而 节省 内 存 。 

如 图 8.20 所 示 的 矩形 图 像 展 示 了 这 种 技术 。 只 需 在 轮 廊 阵 列 中 的 所 有 坐标 上 绘制 一 个 圆圈 
(以 蓝 色 绘 制 )。 第 一 张 图 片 显 示 了 用 cvCHAIN_APPROX_NONE (734 点 ) 获得 的 点 数 ; 第 二 张 
图 片 显示 了 带 有 cv.CHAIN_APPROX_SIMPLE (4X 4 点 ) 的 点 数 。 看 ， 它 节省 了 多 少 内 存 ! 
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图 8.20 图 的 轮廓 近似 


(1) 图 像 矩 。 
图 像 矩 可 以 用 来 计算 物体 的 质心 、 面 积 等 特征 。 函 数 cv.moments() 给 出 了 一 个 由 所 有 计算 
得 到 的 矩 值 的 字典 ， 


| import numpy as np 


import cv2 as cv 

| img = cv.imread('star.jpg' ,0) 

ret,thresh = cv.threshold(img,127,255,0) 
contours,hierarchy = cv.findContours(thresh, 1, 2) 
| cat = contours [0] 


M = cv.moments(cnt) 


| print ( M ) 


从 这 些 图 像 矩 中 ， 可 以 提取 有 用 的 数据 ， 如 面积 、 质 心 等 。 质 心 由 关系 给 出 ，cx = ml0/m00 
All cy = mol/m00。 这 可 以 按 如 下 方式 完成 
cx = int(M['m10']/M['m00']) 


| ey = int(M['m01']/M['m00']) 


(2) 轮廓 区 域 。 
轮 廊 区域 由 函数 cv.contourArea() 或 从 矩 M [m00’] 给 出 。 


‘area = cv.contourArea(cnt) 


(3) 轮廓 周 长 。 
轮廓 周 长 也 被 称 为 弧 长 ， 可 以 使 用 cv.arcLength() 函数 找到 它 。 第 二 个 参数 指定 形状 是 闭合 
轮廓 〈 如 果 传 递 为 True)， 还 是 仅仅 是 曲线 。 


| 
| pacimetur = cv.arcLength(cnt , True) 


(4) 轮廓 逼近 。 
轮廓 逼近 根据 指定 的 精度 将 轮廓 形状 近似 为 具有 较 少 顶点 数 的 另 一 个 形状 。 它 是 Douglas- 
Peucker 算法 的 一 种 实现 方式 。 查 看 维基 百科 页 面 以 获取 算法 和 演示 。 
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要 理解 这 一 点 ， 假 设 你 试图 在 图 像 中 找到 一 个 正方 形 ， 但 由 于 图 像 中 的 某 些 问题 ， 你 没有 
得 到 一 个 完美 的 正方 形 ， 而 是 一 个 “ 坏 形状 ”， 如 图 8.21(a) 所 示 。 现 在 你 可 以 使 用 此 功能 获得 
近似 形状 。 在 此 ， 第 二 个 参数 称 为 epsilon， 它 是 从 轮廓 到 近似 轮廓 的 最 大 距离 。 这 是 一 个 准确 
度 参数 。 需 要 明智 的 选择 epsilon 才能 获得 正确 的 输出 。 


epsilon = 0.1*cv.arcLength(cnt,True) 


approx = cv.approxPolyDP (cnt,epsilon,True) 


在 图 8.21(b) 中 ， 绿 线 表 示 epsilon = MICH 10% 的 近似 曲线 。 图 8.21(c) 显示 相同 的 
epsilon = 弧 长 的 1%。 第 三 个 参数 指定 曲线 是 否 关闭 。 


(b) 
8.21 图 的 轮廓 逼近 


(5) (hate 

凸 壳 看 起 来 类 似 于 轮廓 近似 , 但 事实 并 非 如 此 (两 者 在 某 些 情况 下 可 能 会 提供 相同 的 结果 )。 
iX H, cv.convexHull() 函数 检查 昌 线 是 否 存在 凸 性 缺陷 并 进行 修正 。 一 般 而 言 ， 凸 曲线 是 有 凸 出 
或 至 少 平坦 的 曲线 。 如 果 它 在 内 部 膨胀 ， 则 称 为 凸 性 缺陷 。 例 如 ， 检 查 如 图 8.22 所 示 的 手 形 图 
像 。 红 线 表 示 手 的 凸 包 。 双 面 箭 头 标 记 显 示 凸 起 缺 哆 ， 即 船体 与 轮廓 的 局 部 最 大 偏差 。 

有 一 些 情况 要 讨论 它 的 语法 : 


hull = cv.convexHull(points[, hull[, clockwise[, returnPoints]] | 


图 8.22 图 的 凸 性 


参数 详情 : 
points: 传 入 的 轮廓 。 
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hull: 输出 ,通常 不 需要 。 

clockwise: 方向 标志 。 如 果 为 Trne， 则 输出 凸 包 为 顺 时 针 方 向 ， 否 则 ， 它 为 逆 时 针 方 向 。 

returnPoints: 默认 为 True， 然 后 它 返 回 壳 的 坐标 。 如 果 为 False， 则 返回 与 船体 点 对 应 的 轮 
廓 点 的 索引 。 

因此 ， 要 获得 图 8.22 所 示 的 凸 包 ， 以 下 就 足够 了 : 


hull = cv.convexHull (cnt) 


但 是 , 如 果 想 找到 凸 性 缺陷 , 需要 传递 returnPoints = False。 为 了 理解 它 , 我 们 将 采用 上 面 的 
BAR. 首先 , 发 现 它 的 轮廓 为 cnt. WERE HA returnPoints = True, 得 到 值 为 [[[234 
202]]、[[51 202]. [[51 79]]. [[234 79]]] 的 这 4 个 角落 和 矩形 点 。 现 在 如 果 使 用 returnPoints = False 
做 同样 的 事情 ， 得 到 的 结果 是 [[129]、[67]、[0]、[142]]。 这 些 是 轮廓 中 对 应 点 的 索引 。 例 如, 检 
查 第 一 个 值 cnt [129] = [[234,202]]， 它 与 第 一 个 结果 相同 ， 依 此 类 推 。 

当 我 们 讨论 西 性 缺陷 时 ， 会 再 次 看 到 它 。 

(6) 检查 凸 性 。 

函数 cv.isContourConvex() 的 功能 是 可 以 检查 曲线 是 否 凸 起 。 它 只 返回 True 或 False. 


k = cv.isContourConvex(cnt) 


(7) 边界 矩形 。 

边界 矩形 有 直 边 珑 形 和 旋转 矩阵 两 种 类 型 。 

O 直 边 矩形 。 它 是 一 个 直 边 的 矩形 ， 不 考虑 对 象 的 旋转 。 因 此 ， 边 界 矩 形 的 面积 不 会 最 小 。 
它 由 函数 cv.boundingRect() 找到 。 

设 (z，y) ABN EfüAbs, Cu, h) 为 宽度 和 高 度 。 


x,y,w,h = cv.boundingRect (cnt) 


cv.rectangle (img, (x,y), (x*w,y*h), (0,255,0) ,2) 


@ 旋转 矩形 。 这 里 以 最 小 面积 绘制 边界 矩形 并 考虑 旋转 。 使 用 的 函数 是 cv.minAreaRect(), 
它 返回 一 个 Box2D 结构 ， 其 中 包含 detals - (center(x. y). (width, height), rotation of rotation). 
要 绘制 这 个 矩形 ， 需 要 算 形 的 4 个 角 ， 可 通过 函数 cv.boxPoints() 获得 。 


rect = cv.minAreaRect (cnt) 


box = cv.boxPoints (rect) 
box = np.intO(box) 
cv.drawContours (img, [box] ,0,(0,0,255) ,2) 


如 图 8.23 所 示 ， 两 个 矩形 都 显示 在 单个 图 像 中 ， 其 中 绿色 矩形 显示 正常 的 边界 矩形 ， 红 色 
矩形 是 旋转 的 矩形 。 
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图 8.23 i XE RUE RUD 


(8) 最 小 封闭 圈 。 
使 用 函数 cv.minEnclosingCircle() 找到 对 象 的 外 接 圆 。 它 是 一 个 圆圈 ， 以 最 小 的 面积 完全 履 
mW, WE 8.24 Pras. 


(x,y),radius = cv.minEnclosingCircle(cnt) 


center = (int(x),int(y)) 
radius = int (radius) 


cv.circle(img,center,radius, (0,255,0) ,2) 


EN 


图 8.24 最 小 封闭 图 


(9) 拟 合 椭圆 。 
拟 合 椭圆 即将 椭圆 拟 合 到 一 个 物体 上 。 它 返 回 刻 有 椭圆 的 旋转 矩形 ， 如 图 8.25 所 示 。 


ellipse = cv.fitEllipse(cnt) 


cv.ellipse(img,ellipse,(0,255,0),2) ` | 


* 


图 8.25 拟 合 椭圆 示意 图 


(10) 拟 合 一 条 线 。 
同样 ， 可 以 在 一 组 点 上 拟 合 一 条 线 。 图 8.26 中 包含 一 组 白 点 ， 可 以 近似 直线 。 
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rows,cols = img.shape[:2] 

[vx,vy,x,yl = cv.fitLine(cnt, cv.DIST_L2,0,0.01,0.01) 
lefty = int((-x*vy/vx) + y) 

righty = int(((cols-x)*vy/vx)+y) 

cv.line(img, (cols-1,righty) , (0,lefty) ,(0,255,0) ,2) 


BD 


826 ”直线 拟 合 


宽 高 比 : 对 象 的 边界 矩形 的 宽度 与 高 度 的 比率 。 


AspectRatio = Width / Height 


| X,y,W,h = cv.boundingRect (cnt) 
| 


aspect_ratio = float(w)/h 


范围 : 轮廓 区 域 与 边界 矩形 区 域 的 比率 。 


Extent = ObjectArea / BoundingRectangleArea 


area = cv.contourArea(cnt) 
x,y,W,h = cv. boundingRect (cnt) 
rect_area = w*h 


extent = float (area) /rect_area 


密实 度 (Solidity): 轮廓 区 域 与 其 凸 包 区 域 的 比率 。 


Solidity = ContourArea / ConvexHullArea 


area = cv.contourArea(cnt) 
hull = cv.convexHull (cnt) 
hull area = cv.contourArea(hull) 
solidity - float(area)/hull area 


等 效 直径 : 圆 的 直径 ， 其 面积 与 轮廓 面积 相同 。 
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EquivalentDiameter = 根 号 (4 Œ ContourArea / ) 


area = cv.contourArea(cnt) 


equi diameter = np.sqrt(4*area/np.pi) 


方向 : 对 象 定向 的 角度 。 以 下 方法 还 给 出 了 主轴 和 短 轴 长 度 。 


(x,y), (MA,ma) ,angle = cv.fitEllipse(cnt) 


蒙 版 和 像素 点 : 在 某 些 情况 下 ， 可 能 需要 包含 该 对 象 的 所 有 点 。 代 码 实现 如 下 : 


mask = np.zeros(imgray.shape,np.uint8) 
cv.drawContours (mask, [cnt] ,0,255,-1) 
pixelpoints = np.transpose(np.nonzero (mask) ) 


#pixelpoints = cv.findNonZero(mask) 


这 里 有 两 种 方法 ， 一 种 是 使 用 Numpy 函数 ; 另 一 种 是 使 用 OpenCV 函数 〈 最 后 一 个 注释 
fr) 给 出 相同 的 方法 。 两 种 方法 的 结果 相同 ， 但 形式 略 有 不 同 。Numpy 以 (row, column) 格式 
给 出 坐标 ， 而 OpenCV Lh Cz, y) 格式 给 出 坐标 。 所 以 答案 基本 上 是 互 换 的 。 请 注意 ，row =x 


和 column = y. 


最 大 值 、 最 小 值 及 其 位 置 : 可 以 使 用 掩 模 图 像 找到 这 些 参 数 。 


min_val, max_val, min_loc, max_loc = cv.minMaxLoc(imgray,mask = mask) | 


平均 颜色 或 平均 强度 : 在 这 里 ， 可 以 找到 对 象 的 平均 颜色 。 或 者 它 可 以 是 灰 度 模式 下 对 象 
的 平均 强度 。 我 们 再 次 使 用 相同 的 掩 码 来 完成 它 。 


mean_val = cv.mean(im,mask = mask) | 


极 值 点 : 对 象 的 最 顶部 、 最 底部 、 最 右 侧 和 最 左 侧 的 点 。 


leftmost = tuple(cnt[cnt[:,:,0] .argmin()] [0]) 
rightmost = tuple(cnt[cnt[:,:,0].argmax()] [0]) 
topmost = tuple(cnt[cnt[:,:,1].argmin()] [0]) 
bottommost = tuple(cnt[cnt[:,:,1].argmax()] [0]) 


例如 ， 如 果 将 它 应 用 在 一 个 萎 形 上 CL 8.27)， 会 得 到 以 下 的 结果 。 
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图 8.27 萎 形 的 4 个 极 值 点 


3. 颜色 识别 

OpenCV 中 有 150 多 种 颜色 空间 转换 的 方法 ,但 我 们 只 研究 使 用 最 广泛 两 种 BGR2GRAY 和 
BRG2HSV。 

使 用 函数 cv.cvtColor(input_image, flag) 模式 进行 图 像 的 颜色 转换 空间 ， 其 中 flag 确定 转换 
类 型 。 

进行 BGR/ 灰 度 模 式 转换 ， 使 用 标志 cv.COLOR_BGR2GRAY。 类 似 地 ， 进 行 BGR/HSV 模 
式 转换 ， 使 用 标志 cv.COLOR_BGR2HSV。 要 获取 其 他 标志 ， 只 需 在 Python 终端 运行 以 下 命令 : 


>>> import cv2 as cv 


>>> flags = [i for i in dir(cv) if i.startswith('COLOR_')] 
>>> print( flags ) 


注意 : 对 于 HSV, Hue 范围 是 [0,179]， 饱 和 范围 是 [0,255]， 值 范围 是 [0,255]。 不 同 的 软件 
使 用 不 同 的 规模 。 因 此 ， 如 果 要 将 OpenCV 值 与 它们 进行 比较 ， 则 需要 对 这 些 范 围 进行 标准 化 。 

1) 对 象 跟踪 

我 们 已 经 知道 了 如 何 将 图 像 的 颜色 模式 由 BGR 转换 为 HSV， 我 们 还 可 以 更 进一步 地 利用 
HSV 模式 来 提取 彩色 对 象 。 在 HSV 模式 中 ， 表 示 颜 色 比 在 BGR 颜色 空间 中 更 容易 。 在 我 们 的 
应 用 程序 中 ， 我 们 将 尝试 提取 蓝 色 对 和 象 ， 方 法 如 下 : 

CL) 提取 视频 的 每 一 帧 。 

(2) M BGR 转换 为 HSV 色彩 空间 。 

(3) 将 HSV 颜色 字典 的 数值 设置 为 蓝 色 所 对 应 数值 。 

(4) 单独 提取 蓝 色 对 象 。 

我 们 可 以 对 我 们 想 要 的 图 像 做 任何 事情 。 

以 下 是 注释 详细 的 代码 : 


import cv2 as cv 


import numpy as np 
cap = cv.VideoCapture(0) 
while(1): 

# Take each frame 
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_, frame = cap.read() 
# Convert BGR to HSV 
hsv = cv.cvtColor(frame, cv.COLOR_BGR2HSV) 
# define range of blue color in HSV 
lower blue = np.array([110,50,50]) 
upper blue = np.array([130,255,255]) 
# Threshold the HSV image to get only blue colors 
mask = cv.inRange(hsv, lower blue, upper. blue) 
# Bitwise-AND mask and original image 
res - cv.bitwise and(frame,frame, mask- mask) 
cv.imshow('frame',frame) 
cv.imshow('mask' mask) 
cv.imshow('res',res) 
k = cv.waitKey(5) & OxFF 
if k == 27: 
break 
cv.destroyAllWindows() 


图 8.28 显 示 了 提取 的 蓝 色 物体 。 


tn, HR) csi ST m eB ve tISi- x9 COS 
A828 蓝 色 记 号 笔 识别 


这 是 对 象 跟踪 中 最 简单 的 方法 。 一 旦 你 学 习 了 轮廓 的 功能 ， 就 可 以 做 很 多 事情 ， 比 如 找到 
这 个 物体 的 质心 ， 并 用 它 来 追踪 物体 ， 只 需 在 镜头 前 移动 你 的 手 和 许多 其 他 有 趣 的 东西 来 绘制 
图 表 。 

2) 如 何 找到 要 跟踪 的 HSV fH 

这 是 stackoverflow.com 中 常见 的 问题 。 它 非常 简单 ， 你 可 以 使 用 相同 的 函数 cvcvtColor0)。 
你 只 需 传 递 所 需 的 BGR 值 ， 而 不 是 传递 图 像 。 例 如 ， 要 查找 Green 的 HSV 值 ， 请 在 Python 终 
端 尝试 以 下 命令 ; 
>>> green = np.uint8([[[0,255,0 ]]]) | 
>>> hsv_green = cv.cvtColor(green,cv.COLOR_BGR2HSV) | 
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>>> print( hsv_green ) 
[[[ 60 255 255]]] 


现在 分 别 将 [H-10,100,100] 和 [H + 10,255,255] 作为 下 限 和 上 限 。 除 了 这 种 方法 ， 还 可 以 使 
用 任何 图 像 编辑 工具 (如 GIMP 或 任何 在 线 转 换 器 ) 来 查找 这 些 值 ， 但 不 要 忘记 调整 HSV 的 
范围 。 


8.3 ”综合 应 用 


综合 应 用 DEMO。 

通过 对 以 上 音频 和 视频 处 理 的 了 解 ， 现 在 来 实现 一 个 综合 应 用 DEMO。 

主要 内 容 为 : Roban 机 器 人 被 唤醒 后 , 辨别 并 面向 声 源 处 后 , 向 声 源 处 前 行 , 将 途中 识别 面积 
最 大 的 人 脸 视 为 唤醒 者 , 同时 识别 人 物性 别 和 年 龄 。 走 到 唤醒 者 面前 30~80cm 位 置 时 , 做 基本 问 
候 , 问候 结束 , 紧 接着 进行 自我 介绍 , 介绍 完毕 后 , 进行 水 果 识 别 。 运 行 聊 天 机 器 人 wukong-robot, 
让 Roban 机 器 人 跳舞 ,接着 开始 跳舞 。 

该 综合 应 用 DEMO 文件 路 径 为 : 


/home/lemon/robot_ros_application/catkin_ws/src/ros_actions_node/scripts/integrated_ l 


apply. py 


8.31 BARE 


综合 DEMO 的 工作 原理 : 首先 通过 “灵犀 灵犀 ”唤醒 词 唤醒 Roban 机 器 人 , 当 检测 到 麦克 风 
被 唤醒 的 消息 后 ,机 器 人 会 辨别 声 源 位 置 ， 并 进入 步 态 , 转向 面 对 声 源 位 置 ， 随 后 通过 OpenCV 
进行 人 脸 检 测 ， 识 别 声 源 方向 上 的 最 大 人 脸 ， 并 视 为 唤醒 者 ， 然 后 走 到 唤醒 者 前 的 30~80cm 处 ， 
然后 通过 微软 API 识别 人 脸 属性 ， 如 果 是 0~30 岁 男性 ，Roban 会 说 问候 语 :“ 你 好 ， 小 哥哥 ”; 
MRE 0~30 岁 女 性 ，Roban 会 说 问候 语 :“ 你 好 ， 小 姐姐 ” 如 果 是 31 岁 以 上 的 男性 ，Roban 会 
说 问候 语 :“ 你 好 ， 先 生 ”， 如果 是 31 岁 以 上 的 女性 ，Roban 会 说 问候 语 :“ 你 好 ， 女 士 ”， 随 后 
做 自我 介绍 :“ 我 是 鲁班 ,“ 和 鲁班 ” 源 自 同名 的 古代 创新 工匠 。 我 的 英文 名 叫 Roban。 我 热衷 机 器 
人 学 ， 我 的 身体 配备 了 22 个 能 机 和 丰富 的 传感器 ， 最 擅长 模仿 人 类 行为 。 跳 钴 、 瑜 伽 都 是 我 的 
看 家 本 领 。 我 认识 很 多 水 果 哦 ， 快 来 考 考 我 吧 !” 

然后 进行 水 果 识 别 ， 可 拿 起 人 苹果、 香蕉 、 枪 子 、 梨 、 火 龙 果 放 在 Roban 面前 摄像头 ) 让 
其 识别 ， 如 果 识 别 到 芋 果 ，Roban 会 说 :“ 这 是 苹果 ， 富 含 矿物 质 和 维生素 ， 有 助 于 代谢 掉 体内 
的 多 余 盐 分 ， 苹果 酸 可 代谢 热量 ,防止 下 半身 肥胖 ”如 果 识 别 到 香 榴 ,Roban 会 说 :“ 吃 香花 有 
助 肠 道 消 化 ”如 果 识 别 到 橙子 ，Roban 会 说 :“ 橙 子 富 含 维生素 C， 还 具有 抗 氧化 功能 ， 女 士 多 


raw mw 


WERE REARS”; 如 果 识 别 到 梨 ，Roban 会 说 :“ 这 是 梨 ， 有 润 嗓 去 火 的 功效 ， 如 果 你 是 主播 ， 可 
AER”: 如 果 识 别 到 火龙 果 ，Roban 会 说 :“ 火 龙 果 ， 养 生 爱 好 者 热衷 的 水 果 之 一 ， 几 乎 不 含 
蔗糖 和 果糖 ”。 

跳舞 功能 : 运行 聊天 机 器 人 wukong-robot， 然 后 通过 “灵犀 灵犀 ”唤醒 词 唤 醒 ， 对 Roban 说 
包含 关键 词 〈 跳 舞 、 跳 个 舞 、 跳 支 舞 、 舞 蹈 ) 的 句子 ， 随 后 ，Roban 会 说 :“ 那 我 为 大 家 表演 一 
小 段 ， 希 望 大 家 喜欢 ”， 并 请 求 ROS 跳舞 服务 ， 接 着 开始 跳舞 。 


8.3.2 ”主要 接口 
人 脸 检 测 与 人 脸 识 别 接口 见 表 8.3。 


表 8.3 人 脸 检 测 与 人 脸 识别 接口 


内 容 API 接口 
人 脸 检测 OpenCV Haar Cascade 分 类 器 
人 脸 识别 https://southeastasia.api.cognitive.microsoft.com/face/v 1 .0/verify 
水 果 识别 https://aip.baidubce.com/rest/2.0/image-classify/v 1/classify/ingredient 
ROS 接口 见 表 8.4。 
表 8.4 ROS 接口 


/micarrays/wakeup 


唤醒 机 器 人 
人 脸 检测 服务 
人 脸 识别 服务 
水 果 识 别 服务 


/ros_face_node/face_detect 


/ros_vision_node/face_detect 
/ros_fruit_node/fruit_cognition 


8.3.3 ”运行 方式 
TE ROS 终端 首先 通过 以 下 指令 占用 BodyHub , 使 其 可 控制 舵 机 运动 。 


|rosservice call /MediumSize/BodyHub/StateJump 2 setStatus 


然后 在 终端 运行 python integrated apply.py 即 可 启动 综合 应 用 DEMO 程序 。 


8.4 ”颜色 识别 实践 


8.4.1 HSV 颜色 模型 介绍 


HSV (Hue, Saturation, Value) 颜色 模型 是 根据 颜色 的 直观 特性 由 A.R. Smith 在 1978 年 创建 
的 一 种 颜色 空间 , 也 称 六 角 锥 体 模型 (Hexcone Model)。 这 个 模型 中 颜色 的 参数 分 别 是 色调 CHD. 


FSE ANRE sas 


-一 人 一 


WARE CSO. SEHE CV). 

H: HARRE, WAEA 09—3609, MAL IFRAME i, Af 0°, AE 
Jy 120°, 蓝 色 为 240?。 它 们 的 补 色 是 : EA 60°, WA 180°, 品 红 为 300"; S: 取 值 范围 为 
0.0~1.0; V: 取 值 范围 为 0.0 (Hf) ~1.0 (Af). 

RGB 和 CMY 颜色 模型 都 是 面向 硬件 的 , 而 HSV 颜色 模型 是 面向 用 户 的 。HSV 颜色 模型 的 
三 维 表示 从 RGB 立方 体 演化 而 来 ( 见 图 8.29)。 设 想 从 RGB 沿 立 方 体 对 角 线 的 白色 顶点 向 黑色 
顶点 观察 ， 就 可 以 看 到 立方 体 的 六 边 形 外 形 。 六 边 形 边界 表示 色彩 ， 水 平 轴 表 示 纯 度 ， 明 度 沿 
垂直 轴 测 量 。 


8.29 HSV 颜色 模型 的 三 维 表示 


对 于 基本 色 中 对 应 的 H. S. V 分 量 需 要 给 定 一 个 严格 的 范围 。OpenCV 的 颜色 范围 : H 2g 
0~180; S 为 0~255; V 为 0~255。 


8.4.2 ”识别 小 球 


识别 小 球 ， 可 以 使 用 颜色 识别 ， 也 可 以 使 用 霍 夫 变换 检测 圆 形 的 方法 。 本 节 介 绍 颜色 识别 。 
识别 小 球 的 基本 思路 如 下 : 

(1) 根据 设 定 的 阐 值 , 将 图 像 数 据 进行 颜色 空间 变换 , 通过 将 RGB 颜色 空间 转换 到 HSV Hl 
EZ. 

(2) 使 用 指定 的 HSV 颜色 范围 三 值 化 转换 后 的 图 像 数据 ， 得 到 目标 颜色 区 域 ， 对 目标 颜色 
区 域 进行 团 操作 ， 滤 除 噪声 。 

(3) 对 二 值 图 像 进行 轮廓 查找 ， 若 有 多 个 轮廓 ， 找 到 面积 最 大 的 轮廓 ， 计 算出 轮廓 的 矩 ， 找 
3p. 

颜色 检测 代码 如 下 : 


class ColorObject: 


def __init__(self,lower,upper,cName='none'): 
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self.coLowerColor = lowe 
self.coUpperColor = upper 
self.coResult = {'find':False, 'name':cName} 


def detection(self, image) : 
blurred = cv2.GaussianBlur(image, (5, 5), 0) # 高 斯 模糊 
hsvimg = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 转换 颜色 空间 到 HSV 
mask = cv2.inRange(hsvImg, self.coLowerColor, self.coUpperColor) # 对 图 片 进行 
# 二 值 化 处 理 
mask = cv2.dilate(mask, None, iterations-2) # 膨胀 操作 
mask = cv2.erode(mask, None, iterations-2) # 腐蚀 操作 


contours = cv2.findContours(mask.copy(), cv2.RETR EXTERNAL, cv2.CHAIN_APPROX_ 
SIMPLE) [-2] # 寻找 图 中 轮廓 


self.coResult['find'] = False 
if len(contours) > 0: # 如 果 存 在 至 少 一 个 轮廓 ， 则 进行 如 下 操作 
c = max(contours, key=cv2.contourArea) # 找到 面积 最 大 的 轮廓 
self.coResult['boundingR'] = cv2.boundingRect(c) #x,y,w,h 
if self.coResult['boundingR'] [2]>5 and self.coResult['boundingR'][3]»5: 
M = cv2.moments(c) 
self.coResult['Cx'] = int(M['m10'] / M['m00']) 
self.coResult['Cy'] = int(M['m0i'] / M['m00']) 
self.coResult['contour'] = c 
self.coResult['find'] = True 


return self.coResult 


JE, self.coLowerColor 7j H ARAE (T) (KA; self.coUpperColor 为 目标 颜色 的 高 阔 值 ;self. 
coResult[' Cx ' ] 和 self.coResult[' Cy'] 为 轮廓 矩 心 在 图 像 上 的 坐标 。 
检测 代码 如 下 ; 


# HSV (É 

lowerRed = np.array([i65, 128, 128]) 

upperRed - np.array([180, 224, 255]) 

ball = imgPrcs.ColorÜbject(lowerRed, upperRed) # 定义 小 球 
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result = ball.detection(originImage) # 小 球 检测 
imgPrcs.putVisualization(originlmage, result) # 画 框 


检测 结果 如 图 8.30 所 示 。 


8.30 ”球体 检测 结果 


8.4.3 ”追踪 小 球 


识别 与 定位 小 球 后 ， 便 可 控制 机 器 人 目光 焦点 来 追踪 小 球 ， 基 本 思路 如 下 : 
COD 获取 小 球 在 屏幕 上 的 坐标 ， 分 别 计 算 小 球 坐 标 与 屏幕 中 心 〈 机 器 人 目光 焦距 ) 的 z 距 


BURI y 距离 。 


(2) 将 距离 作为 输入 误差 , 使 用 增 量 式 PID 计算 机 器 人 头 的 俯仰 (pitch) 角 增 量 和 偏转 yaw 


角 增 量 ， 控 制 机 器 人 ， 使 小 球 保持 在 屏幕 中 心 〈 机 器 人 目光 焦距 ) 处 。 


PID 控制 代码 如 下 : 


idList = [21,22] 
valueList = [0,0] 
zAxisLimit, yAxisLimit = 80, 30 


xPid = pidAlg.PositionPID(p-0.05,d-0.0) # x 方向 上 的 pid 
yPid = pidAlg.PositionPID(p=0.04,d=0.0) # y 方 向 上 的 pid 


# HSV Ė 

lowerRed = np.array([165, 128, 128]) 

upperRed = np.array([180, 224, 255]) 

ball = imgPrcs.ColorÜbject(lowerRed, upperRed) # 定义 小 球 


def watchBallLoop(): 


global valueList 


"-——— 


result = ball.detection(originImage) # 小 球 检测 
if result['find'] != False: 
xError = originImage.shape[1]/2.0 - result['Cx'] 
yError = originImage.shape[0]/2.0 - result['Cy'] # 计算 误差 
if (abs(xError) > 4) or (abs(yError) > 4): 
valueList[0] = valueList[0] + xPid.run(xError) 
valueList[1] = valueList[1] + (-yPid.run(yError)) 
# 输出 限 幅 
valueList[0] = valueList[0] > 
valueList[0] = valueList[0] < -zAxisLimit and -zAxisLimit or valueList[0] 
valueList[1] = valueList[1] > yAxisLimit and yAxisLimit or valueList [1] 
valueList[1] = valueList[1] < -yAxisLimit and -yAxisLimit or valueList[1] 
mCtrl.SendJointCommand (nodeControlId, idList, valueList) # 控制 机 器 人 


zAxisLimit and zAxisLimit or valueList [0] 


主 程序 如 下 : 


loopRate = rospy.Rate(10) 


while not rospy.is shutdown(): 
watchBallLoop() 
loopRate.sleep() 


8.4.4 ”追踪 多 种 颜色 小 球 


在 8.4.3 节 的 基础 上 ， 可 实现 同时 追踪 红色 小 球 和 蓝 色 小 球 ， 基 本 思路 如 下 : 

C1) 在 检测 颜色 列表 中 指定 一 个 目标 颜色 。 

(2) 检测 目标 颜色 对 象 。 

(3) 若 检测 到 小 球 ， 输 出 log 信息 ， 并 控制 机 器 人 使 小 球 保持 在 屏幕 中 心 ， 继 续 执 行 步骤 
(2); 若 没有 检测 到 小 球 ， 执 行 步骤 (4). 

(D 目标 颜色 若是 列表 中 的 最 后 一 个 颜色 ， 指 定 列表 中 的 第 1 个 颜色 为 新 的 目标 颜色 ， 否 
则 在 列表 中 指定 当前 颜色 的 下 一 个 颜色 为 新 的 目标 颜色 ， 执 行 步骤 〈2)。 

检测 代码 如 下 : 


tergetColorIndex = 0 


tergetColorList = ['red','blue'] 


tergetColor = tergetColorList [0] 
lastTergetColor = tergetColorList [0] 
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ballList = [] 
ballList.append(imgPrcs.ColorObject(lowerRed, upperRed, tergetColorList[0])) 
ballList.append(imgPrcs.ColorÜbject(lowerBlue, upperBlue, tergetColorList[0])) 


def detectionBall(): 
Elobal tergetColorIndex, tergetColorList, tergetColor, lastTergetColor 
if tergetColor == tergetColorList [tergetColorIndex] : 
result = ballList [tergetColorIndex] .detection(originImage) 
if result['find'] == True: 
if lastTergetColor != tergetColorList [tergetColorIndex] : 
lastTergetColor = tergetColor 
print '%s ball detected'%(tergetColorList [tergetColorIndex]) 
return result 
else: 
lastTergetColor - tergetColor 
tergetColor = tergetColorList [tergetColorIndex < len(tergetColorList)-1 


and tergetColorIndex*1 or 0] 


tergetColorIndex = tergetColorIndex < len(tergetColorList)-1 and tergetColorIndex 
*1 or 0 


return None 


PID 控制 代码 : 


def watchBallLoop(): 
global valueList 
result = detectionBall() 
if result is not None: 


xError = originImage.shape[1]/2.0 - result['Cx'] 


yError = originImage.shape[0]/2.0 - result['Cy'] 

if (abs(xError) > 4) or (abs(yError) > 4): 
valueList[0] = valueList[0] + xPid.run(xError) 
valueList [1] = valueList[1] + (-yPid.run(yError)) 
# output limiting 
valueList [0] 
valueList [0] 
valueList [1] 


M 


valueList[0] > zAxisLimit and zAxisLimit or valueList [0] 


valueList[0] < -zAxisLimit and -zAxisLimit or valueList [0] 


valueList[1] > yAxisLimit and yAxisLimit or valueList[1] 
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valueList[1] = valueList[1] < -yAxisLimit and -yAxisLimit or valueList[1] 


mCtrl.SendJointCommand(nodeControlld, idList, valueList) 


8.5 人 脸 识别 实践 


人 脸 识别 实践 ， 主 要 是 实现 一 个 Roban 机 器 人 头 部 舵 机 跟踪 人 脸 ， 使 人 脸 一 直 处 于 头 部 摄 
像 头 中 间 的 一 个 综合 应 用 。 下 面 给 出 调用 OpenCV 中 基于 Haar 特征 的 人 脸 检测 , 进行 人 脸 跟 踪 
的 示例 。 


1. Harr 特征 


Haar 特征 包含 三 种 : 边缘 特征 、 线 性 特征 和 对 角 线 特征 。 每 种 分 类 器 都 从 图 片 中 提取 出 对 
应 的 特征 。 图 8.31 所 示 为 Haar 特征 包 。 


如 图 8.32 所 示 , 横 的 黑道 将 人 脸 中 较 暗 的 双眼 提取 了 出 来 , 而 竖 的 白道 将 人 脸 中 较 亮 的 鼻梁 


提取 了 出 来 。 


(a) 边缘 特征 


— j 


=i = 
7 t fa 


(c) 对 角 线 特征 
8.31 Haar 特征 包 832 ”和信 脸 特征 提取 


这 种 分 类 器 又 很 像 卷 积 核 。 卷 积 核 也 是 从 图 片 中 提取 指定 特征 的 筛选 器 。 
2. Cascade 级 联 分 类 器 


基于 Haar 特征 的 Cascade 级 联 分 类 器 是 Paul Viola 和 Michael Jone 在 2001 年 的 论文 Rapid 
Object Detection using a Boosted Cascade of Simple Features 中 提出 的 一 种 有 效 的 物体 检测 方法 。 
(1) Cascade 级 联 分 类 器 的 训练 方法 为 Adaboost。 
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(2) 级 联 分 类 器 的 函数 是 通过 大 量 “ 带 人 脸 ” 和 “不 带 人 脸 ” 的 图 片 通过 机 器 学 习 得 到 的 。 
对 于 人 脸 识 别 来 说 ， 需 要 几 万 个 特征 ， 通 过 机 器 学 习 找 出 人 脸 分 类 效果 最 好 、 错 误 率 最 小 的 特 
征 。 训练 开 始 时 ， 所 有 训练 集中 的 图 片 具有 相同 的 权重 ,对 于 被 分 类 错误 的 图 片 , 提升 权重 , E 
新 计算 出 新 的 错误 率 和 新 的 权重 。 直 到 错误 率 或 迭代 次 数 达 到 要 求 ， 这 种 方法 叫 作 “Adaboost”。 

(3) Æ OpenCV 中 可 以 直接 调用 级 联 分 类 器 函数 。 

(4) 将 弱 分 类 器 聚合 成 强 分 类 器 。 最 终 的 分 类 器 是 这 些 弱 分 类 器 的 加 权 和 。 之 所 以 称 之 为 
弱 分 类 器 是 因为 每 个 分 类 器 不 能 单独 分 类 图 片 ， 但 是 将 它们 聚集 起 来 就 形成 了 强 分 类 器 。 论 文 
表明 ， 只 需要 200 个 特征 的 分 类 器 在 检测 中 的 精确 度 达 到 了 95。 

3. 级 联 的 含义 

事实 上 ， 一 张 图 片 绝 大 部 分 的 区 域 都 不 是 人 脸 。 如 果 对 一 张 图 片 的 每 个 角落 都 提取 6000 个 
特征 ， 将 会 浪费 巨 量 的 计算 资源 。 

如 果 能 找到 一 个 简单 的 方法 能 够 检测 某 个 窗口 是 不 是 人 脸 区 域 ， 如 果 该 窗口 不 是 人 脸 区 域 ， 
那么 就 只 看 一 眼 便 直 接 跳 过 ， 也 就 不 用 进行 后 续 处 理 了 ， 这 样 就 能 集中 精力 判别 那些 可 能 是 人 
脸 的 区 域 。 为 此 ， 有 人 引入 了 Cascade 分 类 器 。 它 不 是 将 6000 个 特征 都 用 在 一 个 窗口 ， 而 是 将 
特征 分 为 不 同 的 阶段 ， 然 后 一 个 阶段 一 个 阶段 地 应 用 这 些 特征 (通常 情况 下 ， 前 几 个 阶段 只 有 
很 少量 的 特征 ) 。 如 果 窗 口 在 第 一 个 阶段 就 检测 失败 了 ， 那 么 就 直接 舍弃 它 ， 无 须 考虑 剩 下 的 
特征 。 如 果 检 测 通 过 ， 则 考虑 第 三 阶段 的 特征 并 继续 处 理 。 如 果 所 有 阶段 的 都 通过 了 ， 那 么 这 
个 窗口 就 是 人 脸 区 域 。 作 者 的 检测 器 将 6000+ 的 特征 分 为 了 38 个 阶段 ， 前 5 个 阶段 分 别 有 1、 
10. 25、25、50 个 特征 《前 文 图 中 提 到 的 识别 眼睛 和 鼻梁 的 两 个 特征 实际 上 是 由 Adaboost 中 
得 到 的 最 好 的 两 个 特征 )。 根 据 作 者 所 述 ， 平 均 每 个 子 窗口 只 需要 使 用 6000+ 个 特征 中 的 10 个 
左右 。 

本 例 程 的 人 脸 特征 文件 位 于 


> '/home/lemon/robot_ros_application/catkin_ws/src/ros_actions_node/scripts/tracking/ 


haarcascade frontalface alt2.xml' 


4. PID 控制 算法 

PID 控制 是 一 种 线性 调节 器 ， 它 将 目标 值 Setpoint 与 实际 输出 值 Output 的 偏差 的 比例 CP. 
BASS CD. tod CD) 通过 线性 组 合 构成 控制 量 ， 对 控制 对 象 进行 控 制 。 

1) PID 调节 器 各 环节 的 作用 

《1) 比例 环节 : 即时 成 比例 地 反映 控制 系统 的 偏差 信号 e(t), 偏差 一 旦 产生 ， 调 节 器 立即 产 
生 控 制作 用 以 减 小 偏差 。 

(2) 积分 环节 : 主要 用 于 消除 静 差 , 提高 系统 的 无 差 度 。 积分 作用 的 强 弱 取决 于 积分 时 间 常 
JT DRAK, REAR, UERR 
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QD 微分 环节 : 能 反映 偏差 信号 的 变化 趋势 (变化 速率 )， 并 能 在 偏差 信号 的 值 变 得 太 大 之 
前 ， 在 系统 中 引入 一 个 有 效 的 早期 修正 信号 ， 从 而 加 快 系统 的 动作 速度 ， 减 小 调节 时 间 。 
图 8.33 所 示 为 PID 控制 系统 框图 。 


8.33 PID 控制 系统 框图 


2) PID 控制 的 形式 
(1) 位 置式 PID 控制 


公式 : 


u(t) = Kp ew + F j EE | 


T d(t) 

AP, u(t) 为 控制 系统 的 输出 ; el(t) 为 控制 系统 的 输入 ,一般 为 设 定量 与 被 控 量 的 差 ， 即 elt) = 
r(t) 一 c(t); Kp 为 控制 系统 的 比例 系数 ; T 为 控制 系统 的 积分 时 间 ; Ta 为 控制 系统 的 微分 时 间 ; 
了 为 系统 采样 周期 。 

由 于 计算 机 控制 是 一 种 采样 控制 ， 它 只 能 根据 采样 时 刻 的 偏差 值 计算 控制 量 , 在 计算 机 控制 
系统 中 ，PID 控制 规律 的 实现 必须 用 数值 逼近 的 方法 , 当 采 样 周 期 相当 短 时 ， 用 求 和 代替 积分 、 
用 后 向 差分 代替 微分 ， 使 模拟 PID 离散 化 变 为 差分 方程 。 

离散 化 公式 : 


FE HE T 
u(k) = Kp e(t) + Delt) + 7 lelk) — elk -1)] 
! i=1 


位 置式 PID 控制 算法 ， 适 用 于 不 带 积分 元 件 的 执行 器 ， 执 行 器 的 动作 位 置 与 其 输入 信号 呈 
一 一 对 应 的 关系 。 控 制 器 根据 第 次 被 控 变 量 采样 结果 与 设 定 值 之 间 的 偏差 e(k) 计算 出 第 大 次 
采样 之 后 所 输出 的 控制 变量 。 

(2) 增 量 式 PID 控制 

增 量 式 PID 控制 算法 ， 即 输出 量 为 控制 量 的 增 量 (用 Au(k) 表示 ) 控制 系统 。 算 法 在 应 用 
时 , 输出 的 控制 量 Aulk) 相对 的 是 本 次 实行 设备 的 位 置 增 量 , 并 非 相 对 实行 设备 的 现实 位 置 , 所 
以 该 算法 需要 实行 设备 应 该 对 控制 量 增 量 进行 累积 ， 才 能 实现 对 被 控 系 统 的 控制 。 由 Au(k) = 
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u(k) — u(k — 1) 可 得 到 : 
Au(k) = K, few kenya Telk) T Ta lek) E oe E a1} 


增 量 式 PID 控制 中 没有 累加 环节 ， 不 用 大 量 的 计算 , 控制 增 量 值 Aul) 与 系统 最 近 的 三 次 
的 采样 值 有 关 ，, 方便 使 用 加 权 处 理 达 到 良好 的 控制 效果 ; 每 次 计算 机 输出 的 仅仅 是 控制 增 量 , 即 
相对 执行 设备 位 置 的 改变 量 。 

常用 的 控制 方式 : 

(OD P Bl: u(k) = Kp- elt) + uo 

(2) PI 控制 : u(k) = Kp- e(t) + Kp- T È e(i) + uo 

i=l 
(3) PD 控制 : u(k) = Ky elt) + Kp- B[e(k) — e(k — 1)] + uo 
(4) PID 控制 : u(k) = Kp elt) + Kp F : e(i) + Ky : T2 [e(k) — elk — 1)] + uo 
i=1 


= 


5. 人 脸 跟踪 的 示例 基本 原理 


首先 订阅 头 部 摄像 头发 布 的 话题 消息 ， 获 取 RGB 图 像 ， 然 后 将 图 片 输入 Haar 特征 分 类 
器 进行 人 脸 检 测 ， 返 回 图 片 中 所 有 人 脸 和 矩形 的 左上 和 角 坐 标 和 宽 高 信息 ， 通 过 返回 的 信息 ， 计 算 
人 脸 矩 形 的 面积 ， 得 到 面积 最 大 的 矩形 对 应 的 人 脸 作为 跟踪 目标 ， 然 后 计算 人 脸 坐 标 与 图 片 中 
心 点 坐标 的 距离 ， 通 过 PID 控制 算法 得 到 下 发 给 舵 机 的 角度 值 ， 并 下 发 指令 控制 舵 机 做 出 相应 
运动 。 

在 Roban 机 器 人 人 脸 跟踪 案例 中 〈( 见 图 8.34)， 首 先 会 从 整 张 图片 中 检测 人 脸 ， 当 检测 到 人 
脸 后 ， 保 存 目 标 人 脸 作 为 模板 ， 以 后 会 在 当前 人 脸 区 域 的 两 倍 范围 内 继续 检测 ， 如 果 偶 尔 检测 
人 脸 失 败 时 ， 这 时 会 用 到 OpenCV 中 的 模板 匹配 函数 : matchTemplate， 将 之 前 保存 的 人 脸 模板 


T 


pan right 


y Center(w/2, h/2) 


distance ^... 


8.34 Roban 机 器 人 人 脸 跟 踪 
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与 图 片 进行 匹配 ， 得 到 人 脸 信 息 ， 随 后 继续 在 当前 人 脸 两 倍 范围 内 检测 ， 如 果 检 测 人 脸 失败 超 
时 ， 则 会 继续 从 整 张 图 片 中 检测 人 脸 。 程 序 会 一 直 运 行 ， 直 到 收 到 终止 指令 。 

从 相机 获取 的 RGB 图 像 ， 由 于 图 片 大 小 固定 ， 所 以 图 像 中 点 坐标 是 固定 的 ， 机 器 人 头 部 由 
两 个 舵 机 控制 , 一 个 负责 水 平 旋转 (左右 摇头 ), 一 个 负责 上 下 旋转 (上 下 点 头 )， 计 算 人 脸 中 点 
到 图 片 中 点 的 水 平 距离 (pan right) 和 垂直 距离 (tilt down)， 然 后 分 别 计 算 两 个 舵 机 的 步 进 值 ， 
从 而 控制 舵 机 跟踪 人 脸 。 

示例 代码 如 下 : 


#!/usr/bin/Python 
import sys 


import os 

import cv2 

import signal 

import Queue 

import rospy 

from sensor_msgs.msg import Image 

from cv_bridge import CvBridge, CvBridgeError 
import array 

import time 

import threading 

from bodyhub.srv import * # for SrvState.srv 
from bodyhub.msg import JointControlPoint 
from lejulib import * 


SERVO = client_action.SERVO 

QUEUE IMG = Queue.Queue(maxsize-2) 

bridge = CvBridge() 

faceadd = "/home/lemon/robot. ros. application/catkin ws/src/ros actions. node/scripts/ 
tracking/haarcascade frontalface alt2.xml" 


face detector = cv2.CascadeClassifier(faceadd) 


class FaceConfig: 
def . init. (self): 
self.running - True 


self.size - 0.5 
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self. 
self. 
self 
self 
self. 
self 
self 
self. 
self 
self 
self 
self. 
self. 
self 


face = 0, 0, 0, 0 


face_roi = 0, 0, 0, 0 


.face template = None 


.found face = False 


template_matching_running = False 


.template matching start time = 0 


.template matching current time = 0 


center x - 160 


.center y - 120 


.pan = 0 
.tlt = 0 
error pan = 0 


error tlt = 0 


.HeadJointPub = rospy.Publisher('MediumSize/BodyHub/HeadPosition', 


JointControlPoint, queue. size-100) 


Face = FaceConfig() 


def doubleRectSize(input rect, keep inside): 


xi, yi, wi, hi = input. rect 


xk, yk, wk, hk = keep inside 


wo 
ho 


xo 


yo 


if 


if 


if 


if 


=wi * 2 
= hi * 2 
= xi- wi // 2 
miyit- ni 1//02 


wo > wk: 
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if xo + wo > wk: 
xo = wk - wo 
if yo + ho > hk: 
yo = hk - ho 


return xo, yo, wo, ho 


def face_size(face): 
X, y, W, h = face 


return w * h 


def face filter(face list): 
face.size list = map(face size, face list) 
target index = face, size list.index(max(face. size list)) 


return face list[target index] 


def detectFaceAllSizes(frame): 


"""Detect using cascades over whole image 


:param frame: 
:return: 
"nn 
gray = cv2.cvtColor(frame, cv2.COLOR. BGR2GRAY) 
face locations = face detector.detectMultiScale( 
gray, scaleFactor-1.1, minNeighbors-3, minSize-(frame.shape[1] / 12, frame. 
shape[0] / 12), 
maxSize-(2 * frame.shape[i1] / 3, 2 * frame.shape[1] / 3)) 
if len(face locations) <= 0: 
Face.face = 0, 0, 0, O 
return 
Face.found face - True 
Face.face = face filter(face locations) 
| Pere fup terete = frame[Face.face[1]:(Face.face[1] + Face.face[3]), 
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Face.face[0]: (Face.face[0] + Face.face[2])].copy() 


Face.face_roi = doubleRectSize(Face.face, (0, 0, frame.shape[1], frame.shape[0])) 


def detectFaceAroundRoi (frame) : 


"""Detect using cascades only in ROI 


‘param frame: 
:return: 
won 
face tem = frame[Face.face_roi[i]:Face.face_roi[i] + Face.face roi[3], 
Face.face roi[0]:Face.face roi[0] + Face.face roi[2]] 
gray = cv2.cvtColor(face tem, cv2.COLOR_BGR2GRAY) 
face locations = face, detector.detectMultiScale( 
gray, scaleFactor=1.1, minNeighbors=3, minSize-(frame.shape[i] / 12, frame. 
shape[0] / 12), 
maxSize-(2 * frame.shape[i] / 3, 2 * frame.shape[1] / 3)) 
if len(face locations) «- 0: 
Face.template matching running - True 
if Face.template matching start time == 
Face.template matching start time = cv2.getTickCount () 
return 
Face.template matching running - False 
Face.template, matching current time = 0 


Face.template matching start time = 0 


Face.face = face filter(face locations) 

Face.face[0] += Face.face_roi[0] 

Face.face[1] += Face.face_roi[1] 

Face.face template = frame[Face.face[i]:Face.face[1] + Face.face[3], 


Face.face[0]:Face.face[0] + Face.face[2]].copy O 


Face.face roi = doubleRectSize(Face.face, (0, 0, frame.shape[i], frame.shape[0])) 


338 4l 仿 人 机 器 人 建 模 与 控制 


def detectFacesTemplateMatching(frame): 


"""Detect using template matching 


:param frame: 
:return: 
"nu 
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 
Face.template matching current time - cv2.getTickCount() 
duration = (Face.template matching current time - Face.template matching start. 
time) / cv2.getTickFrequency() 
if duration > 1: 
Face.found face - False 
Face.template matching running = False 
Face.template matching start time = 0 
Face.template matching current time = 0 
target = gray[Face.face.roi[1]:Face.face roi[1] + Face.face roi[3], 
Face.face_roi[0]:Face.face_roi[0] + Face.face_roi[2]] 
Face.face_template = cv2.cvtColor(Face.face_template, cv2.COLOR_BGR2GRAY) 
res = cv2.matchTemplate(target, Face.face_template, cv2.TM_CCOEFF) 
min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) 
max x = max_loc[0] + Face.face. roi[0] 


max_y = max loc[1] + Face.face_roi[1] 


Face.face - max x, max y, Face.face[2], Face.face[3] 
Face.face template = frame[Face.face[1]:Face.face[1] + Face.face[3], 
Face.face[0]:Face.face[0] + Face.face[2]].copyO 


Face.face roi = doubleRectSize(Face.face, (0, 0, frame.shape[1], frame.shape[0])) 


def show face(face): 
face cx = (face[0] + face[2] / 2) / Face.size 
face. cy = (face[1] + face[3] / 2) / Face.size 


client label.set camera label((255, 0, 0), (face cx, face.cy), face[2]/Face.size, 


face[3]/Face.size) 


def detectFace(): 
rate - rospy.Rate(100) 
while not Face.found face and Face.running: 
time.sleep(0.01) 
if not QUEUE. IMG.empty(): 
frame - QUEUE. IMG.get () 
else: 
continue 
detectFaceAllSizes(frame) 


show. face(Face.face) 


while Face.found face and Face.running: 

rate.sleep() 

if not Face.face_template.any(): 
continue 

if not QUEUE_IMG.empty(): 
frame = QUEUE IMG.get() 

else: 
continue 

detectFaceAroundRoi (frame) 

if Face.template matching running: 
detectFacesTemplateMatching(frame) 


show face(Face.face) 


def async do. job(func): 
async = threading.Thread(target-func) 
async.setDaemon(True) 


async.start() 


def set head servo(angles): 


"""set head servos angle 


:param angles: [pan, tilt] 
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:return: 

nn" 

angles - array.array("d", angles) 
Face.HeadJointPub.publish(positions-angles, mainControlID-2) 
time.sleep(0.01) 


def terminate(data): 
"""Terminate all threads 
now 
rospy.loginfo(data, data) 


Face.running = False 


def thread_face_center(): 
while Face.running: 
time.sleep(0.01) 
face_x = Face.face[0] + Face.face[2] / 2 
face y = Face.face[i] + Face.face[3] / 2 


if face x == 0 and face. y == 0: 
face x - Face.center x 
face. y = Face.center y 
Face.error pan - Face.center x - face x 
Face.error tlt - Face.center y - face y 
rospy.logdebug("Face.error pan,Face.error tlt %f,%f", Face.error pan, Face. 
error tlt) 
def thread, set, servos(): 
set head, servo([Face.pan, Face.tlt]) 
step - 0.01 
while Face.running: 
if abs(Face.error pan) > 15 or abs(Face.error tlt) > 15: 
if abs(Face.error_pan) > 15: 
Face.pan += step * Face.error.pan 
if abs(Face.error tlt) > 15: 


Face.tlt += step * Face.error tlt 
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if Face.pan > 90.0: 
Face.pan = 90.0 

if Face.pan < -90.0: 
Face.pan = -90.0 

if Face.tlt > 45.0: 
Face.tlt = 45.0 

if Face.tlt < -45.0: 
Face.tlt = -45.0 

set head, servo([Face.pan, -Face.tlt]) 

else: 
time.sleep(0.01) 


def image callback(msg): 
try: 
cv2 img = bridge.imgmsg to. cv2(msg, "bgr8") 
except CvBridgeError as err: 


print(err) 
else: 
cv2 img - cv2.resize(cv2 img, (0, 0), fx-Face.size, fy-Face.size) 
if QUEUE. IMG.fu110: 
QUEUE. IMG.get () 
QUEUE. IMG.put(cv2 img, block=True) 


def main(): 
try: 

rospy.init node("face tracking", anonymous-True) 

rospy.sleep(0.2) 

client controller.send video status(True, "/camera/label/image raw" ,width-640, 
height-480) 

image topic = "/camera/color/image raw" 

rospy.Subscriber(image topic, Image, image. callback) 


rospy.Subscriber('terminate current process', String, terminate) 


udis oce 


async. do, job(detectFace) 

async. do, job(thread, face center) 
async, do. job(thread, set. servos) 
while Face.running: 


time.sleep(0.01) 


except Exception as err: 
serror(err) 
finally: 
client, controller.send, video, status(False, "/camera/label/image raw",width 
=640 ,height=480) 
SERVO .HeadJointTransfer( [0,0] ,time=1000) 
SERVO ..MotoWait () 


finishsend() 
if | name, == '__main__': 
main() 
代码 解析 : 


# 首先 导入 程序 所 需要 的 库 和 ROS 相 关 的 消息 ， 

SERVO = client_action.SERVO 

QUEUE IMG = Queue. Queue (maxsize=2) 

bridge = CvBridge() 

faceadd = "/home/lemon/robot_ros_application/catkin_ws/src/ros_actions_node/scripts/ 
tracking/haarcascade, frontalface, alt2.xml" 

face. detector = cv2.CascadeClassifier(faceadd) 

# 然后 定义 动作 执行 实例 SERVO, 

# 定义 存储 图 像 的 队列 QUEUE_IMG， 图 像 转 换 实例 bridge, 

# 定义 基于 Haar 特征 的 人 脸 检 测 分 类 器 face_detector 


Face = FaceConfig() 

# 定义 一 个 人 脸 参 数 类 ， 用 于 处 理 人 脸 跟 踪 中 各 种 参数 变化 ， 以 及 舵 机 值 下 发 

# Face.size: 处 理 图 片 的 比例 ，Face.face: 存 储 识 别 到 的 最 大 人 脸 区 域 ， 

# Face.face_roi:; 存 储 识别 到 人 脸 的 两 倍 区 域 ，Face .face_template: 存 储 人 脸 模板 ， 
# Face.HeadJointPub: 下 发 舱 机 值 ， 控 制 稻 机 运动 
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— 
doubleRectSize(input_rect, keep_inside) 


# doubleRectSize 函数 ， 返 回 当 前 人 脸 区 域 的 两 倍 的 一 个 区 域 


def detectFaceAllSizes(frame): 


"""Detect using cascades over whole image 


:param frame: 
:return: 
"nn 
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 
face_locations = face_detector.detectMultiScale( 
gray, scaleFactor=1.1, minNeighbors-3, minSize-(frame.shape[i] / 12, frame. 
shape[0] / 12), 
maxSize-(2 * frame.shape[i] / 3, 2 * frame.shape[1] / 3)) 
| if len(face locations) <= 0: 
Face.face = 0, 0, 0, 0 
return 
Face.found face = True 
Face.face - face filter(face locations) 
Face.face template = frame[Face.face[1]:(Face.face[1] + Face.face[3]), 
Face.face[0]:(Face.face[0] + Face.face[2])].copyO 
Face.face roi = doubleRectSize(Face.face, (0, 0, frame.shape[i], frame.shape[0])) 
# 首先 使 用 cv2.cvtColor 进行 灰 度 处 理 
# 从 整 张 图 片 中 检测 人 脸 ， 使 用 基于 Haar 特征 的 人 脸 检 测 分 类 器 face detector 来 进行 人 脸 检 测 
# Face.face = face filter(face locations) 返回 最 大 人 脸 区 域 赋 给 Face.face 
# 然后 将 人 脸 区 域 生成 模板 赋 给 Face.face_template 
# 同时 确定 使 用 doubleRectSize 函数 ， 确 定 人 脸 区 域 两 倍 的 区 域 ， 用 于 后 面 检测 


def detectFaceAroundRoi(frame): 
"""Detect using cascades only in ROI 


:param frame: 


‘return: 


face tem = frame[Face.face roi[1]:Face.face roi[1] + Face.face roi[3], 
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Face.face_roi[0]:Face.face_roi[0] + Face.face_roi[2]] 
gray = cv2.cvtColor(face_tem, cv2.COLOR_BGR2GRAY) 
face_locations = face_detector.detectMultiScale( 

gray, scaleFactor-1.1, minNeighbors=3, minSize=(frame.shape[1] / 12, frame. 
shape[0] / 12), 
maxSize-(2 * frame.shape[i] / 3, 2 * frame.shape[i] / 3)) 
if len(face locations) «- 0: 
Face.template matching running - True 
if Face.template matching start time == 0: 
Face.template matching start time - cv2.getTickCount() 
return 
Face.template matching running - False 
Face.template matching current time - O 


Face.template matching start time - 0 


Face.face = face filter(face locations) 


Face.face[0] += Face.face roi[0] 


Face.face[1] += Face.face roi[1] 
Face.face template = frame[Face.face[1]:Face.face[i] + Face.face[3], 
Face.face[0]:Face.face[0] + Face.face[2]].copyO 
Face.face roi = doubleRectSize(Face.face, (0, 0, frame.shape[i], frame.shape[0])) 
# 从 人 脸 区 域 两 倍 的 区 域 中 进行 人 脸 检测 ， 可 提高 检测 效率 
# 同上 将 新 检测 的 人 脸 区 域 赋 给 Face.face， 同 时 更 新 生成 模板 
# 更 新 人 脸 区 域 两 倍 的 区 域 


def detectFacesTemplateMatching(frame):. 
"""Detect using template matching 


:param frame: 

:return: 

"nun 

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 
Face.template matching current time = cv2.getTickCount () 


duration - (Face.template matching current time - Face.template matching start. 


time) / cv2.getTickFrequency () 


UE E la 


if duration > 1: 
Face.found_face = False 
Face.template_matching_running = False 
Face.template matching start time = 0 
Face.template matching current time - 0 
target - gray[Face.face roi[1]:Face.face roi[1] * Face.face roi[3], 
Face.face roi[0]:Face.face roi[0] + Face.face_roi[2]] 
Face.face, template = cv2.cvtColor(Face.face template, cv2. COLOR, BGR2GRAY) 
res = cv2.matchTemplate(target, Face.face template, cv2.TM_CCOEFF) 
min val, max val, min loc, max loc - cv2.minMaxLoc(res) 


max x = max loc[0] + Face.face_roi[0] 


max y = max loc[1] + Face.face roi[1] 


Face.face = max x, max y, Face.face[2], Face.face[3] 


Face.face template = frame[Face.face[1]:Face.face[1] + Face.face[3], 
Face.face[0]:Face.face[0] + Face.face[2]].copyO 
Face.face roi - doubleRectSize(Face.face, (0, 0, frame.shape[1], frame.shape[01)) 
# RERA AHE ERA EH DC 3l, p d (rA MAK, WA RR Bo £1 A, de o 
# if duration > 1: 这 里 有 一 个 判断 ， 当 从 人 脸 区 域 两 倍 的 区 域 中 进行 人 脸 检 测 失败 的 时 间 超 过 1s， 
# 则 返回 从 整 张 图 片 中 检测 人 脸 
# 同上 将 新 检测 的 人 脸 区 域 赋 给 Face.face， 同 时 更 新 生成 模板 ， 更 新 人 脸 区 域 两 倍 的 区 域 


def detectFace(): 
rate = rospy.Rate(100) 
while not Face.found_face and Face.running: 
time.sleep(0.01) 
if not QUEUE_IMG.empty(): 
frame = QUEUE. IMG.get () 


else: 


continue 

detectFaceAllSizes(frame) 

show. face(Face.face) 

while Face.found face and Face.running: 
rate.sleep() 


if not Face.face_template.any(): 
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Continue 
if not QUEUE_IMG .empty() : 
frame = QUEUE_IMG.get() 
else: 
continue 
detectFaceAroundRoi (frame) 
if Face.template_matching_running: 
detectFacesTemplateMatching (frame) 
show_face(Face.face) 
# 用 于 人 脸 检测 的 函数 ， 首 先 会 从 整 张 图 片 中 检测 人 有 验 ， 当 检测 到 人 脸 后 ， 保 存 目 标 大 脸 作为 模板 
# 以 后 会 在 当前 人 脸 区 域 的 两 倍 范围 内 继续 检测 ， 
# 如 果 偶 尔 检测 人 脸 失败 时 ， 会 用 模板 匹配 得 到 人 脸 信 息 ， 随 后 继续 在 当前 人 脸 两 入 范围 内 检测 
# 如 果 检 测 人 脸 失 败 超时 ， 则 会 继续 从 整 张 图 片 中 检测 人 有 验 。 程 序 会 一 直 运 行 ， 直 到 收 到 终止 指令 


def thread_face_center(): 


while Face.running: 


time.sleep(0.01) 
face_x = Face.face[0] + Face.face[2] / 2 
face y = Face.face[1] + Face.face[3] / 2 
if face x == 0 and face y == 0: 
face x - Face.center x 
face y = Face.center y 
Face.error pan - Face.center x - face x 
Face.error tlt - Face.center y - face y 
rospy.logdebug("Face.error pan,Face.error tlt %f,7£", Face.error pan, Face. 
error tlt) 
# 用 于 计算 图 像 中 点 与 人 脸 中 的 误差 
# 水 平 误差 : Face.error_pan = Face.center_x - face_x 


# SERŽ: Face.error tlt = Face.center y - face y 


def thread, set, servos(): 
set head servo([Face.pan, Face.tlt]) 
step - 0.01 


while Face.running: 


if abs(Face.error.pan) > 15 or abs(Face.error tlt) > 15: 


— 2 WEN 


hh 


if abs(Face.error_pan) > 15: 
Face.pan += step * Face.error_pan 


i 


Fh 


abs(Face.error tlt) > 15: 


Face.tlt += step * Face.error tlt 


In 


if Face.pan > 90.0: 
Face.pan - 90.0 
if Face.pan < -90.0: 
Face.pan - -90.0 
if Face.tlt > 45.0: 
Face.tlt - 45.0 
if Face.tlt « -45.0: 
Face.tlt - -45.0 
set head servo([Face.pan, -Face.tlt]) 
else: 
time.sleep(0.01) 
# 通过 上 面 计算 的 误差 ， 主 要 通过 PID 中 的 P 控 制 ， 得 到 下 发 稻 机 值 并 下 发 给 舵 机 
# 同时 ， 给 舱 机 设置 一 下 限 位 上 下 为 [-90,90] ,左右 为 [-45,45] 


def image_callback(msg): 
try: 
cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8") 
except CvBridgeError as err: 
print(err) 
else: 
cv2_img = cv2.resize(cv2_img, (0, 0), fx=Face.size, fy=Face.size) 
if QUEUE IMG.fullO: 
QUEUE. IMG.get () 
QUEUE. IMG.put(cv2 img, block-True) 
# 订阅 相机 话题 "/camera/color/image_raw" 的 回调 函数 - 
# 用 于 获取 原始 RGB 图 像 ， 并存 于 QUEUE_IMG 队列 


主要 用 到 的 ROS 消息 见 表 8.5。 
程序 流程 框图 如 图 8.35 所 示 。 
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表 8.5 主要 用 到 的 ROS 消息 


名 称 (topic) 说 阴 
/camera/color/image_raw 获取 RGB 图 像 
/camera/label/image_raw 发 布 标记 后 的 视频 流 
/MediumSize/BodyHub/HeadPosition 下 发 能 机 运动 指令 


摄像 头 、 显 示 窗 口 初始 化 


载 入 级 联 分 类 器 


读 取 每 一 帧 图 像 数据 
图 像 灰 度 处 理 


级 联 分 类 器 对 图 像 做 
多 尺度 检测 


按照 坐标 圈 出 人 脸 


图 8.35 程序 流程 框图 


运行 方式 : 
在 ROS 终端， 首先 通过 以 下 指令 占用 BodyHub , 使 其 可 控制 舵 机 运动 。 


rosservice call /MediumSize/BodyHub/StateJump 2 setStatus 


然后 在 终端 运行 “python face_tracking_2.py” 即 可 启动 人 脸 识 别 跟踪 DEMO 程序 。 


8.6 ”数字 识别 实践 
8.6.1 ”深度 学 习 之 Keras 


Keras 是 一 个 用 Python 编写 的 高 级 神经 网 络 API, 它 能 够 以 TensorFlow. CNTK 或 者 Theano 
作为 后 端 运行 。Keras 的 开发 重点 是 支持 快速 的 实验 。 能 够 在 最 短 时 间 内 把 你 的 想法 转换 为 实验 
结果 ， 是 做 好 研究 的 关键 。 

在 我 们 的 应 用 中 ， 我 们 使 用 TensorFlow 作为 后 端 运行 。 

安装 方式 如 下 : 


pip install keras==2.3.1 
pip install tensorflow==2.1.0 


第 8 章 _ 人 机 交互 i> 349 


1. Keras 简单 使 用 

Keras 的 核心 数据 结构 是 model 一 一 一 种 组 织 网 络 层 的 方式 。 最 简单 的 模型 是 Sequential 顺 
序 模型 ， 它 由 多 个 网 络 层 线性 堆 登 。 对 于 更 复杂 的 结构 ， 应 该 使 用 Keras 函数 式 API， 它 允许 构 
建 任意 的 神经 网 络 图 。 

Sequential 模型 如 下 : 


from keras.models import Sequential 


model = Sequential() 


可 以 简单 地 使 用 .add() SOR BOUM: 


from keras.layers import Dense 


model.add(Dense(units=64, activation='relu', input_dim=100)) 


model.add(Dense(units-10, activation-'softmax')) 


在 完成 了 模型 的 构建 后 , 可 以 使 用 .compile() 来 配置 学 习 过 程 : 


model.compile(loss-'categorical crossentropy', 


optimizer-'sgd', 


metrics-['accuracy']) 


如 果 需 要 ， 还 可 以 进一步 地 配置 优化 器 。Keras 的 核心 原则 是 使 事情 变 得 相当 简单 ， 同 时 又 
允许 用 户 在 需要 的 时 候 能 够 进行 完全 的 控制 (终极 的 控制 是 源 代码 的 易 扩 展 性 )。 


model.compile(loss-keras.losses.categorical crossentropy, 


optimizer-keras.optimizers.SGD(1r-0.01, momentum-0.9, nesterov-True)) 


现在 ， 可 以 批量 地 在 训练 数据 上 进行 迭代 了 : 


# x train 和 y train 是 Numpy 数组 
model.fit(x_train, y_train, epochs=5, batch_size=32) 


只 需 一 行 代码 就 能 评估 模型 性 能 : 


loss_and_metrics = model.evaluate(x_test, y_test, batch_size=128) 


或 者 对 新 的 数据 生成 预测 : 


classes = model.predict(x_test, batch_size=128) 
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2. 构建 数字 识别 模型 

我 们 模型 的 数据 集 采用 MNIST 的 手写 字符 数字 集 ， 训 练 集 为 60 000 张 28x28 像素 的 灰 度 
图 像 ， 测 试 集 为 10 000 同 规格 图 像 ， 总 共 10 类 数字 标签 。 

在 使 用 数字 识别 之 前 , 需要 先 实 现 一 个 能 够 识别 手写 数字 图 片 的 网 络 , 网 络 接 收 数据 时 ， 必 
须 把 一 张 28x28 像素 的 灰 度 图 转换 为 长 784 的 一 维 向 量 。 卷 积 网 络 使 用 如 下 代码 进行 创建 。 


from keras import layers 


from keras import models 


model = models.Sequential() 

model.add(layers.Conv2D(32, (3,3), activation='relu', input_shape=(28,28,1))) 
model .add(layers .MaxPooling2D(2,2)) 

model .add(layers.Conv2D(64, (3,3), activation='relu')) 

| model . add (layers .MaxPooling2D((2,2))) 

model.add(layers.Conv2D(64, (3,3), activation-'relu')) 


model.summary() | 


创建 的 卷 积 网 络 如 图 8.36 所 示 。 


A836 卷 积 网 络 图 


网 络 层 使 用 了 Conv2D 和 MaxPooling, 同时 Conv2D 网 络 层 可 以 直接 接收 二 维 向 量 (28,28,1)， 
这 对 应 的 就 是 手写 数字 灰 度 图 。 卷 积 网 络 的 主要 作用 是 对 输入 数据 进行 一 系列 运算 加 工 ， 它 输 
出 的 是 中 间 形 态 的 结果 。 该 结果 不 能 直接 用 来 做 最 终结 果 ， 而 要 得 到 最 终结 果 ， 需 要 为 上 面 的 
卷 积 网 络 添加 一 层 输出 层 ， 代 码 如 下 : 


model .add(layers.Flatten()) 
model.add(layers.Dense(64, activation='relu')) 
model.add(layers.Dense(10, activation-'softmax')) 


model.summary () | 
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最 终 的 结构 如 图 8.37 所 示 。 


图 8.37 最 终结 构图 


卷 积 网 络 在 最 后 一 层 输出 的 是 (3,3,64) 的 二 维 向 量 ，Flatten() 把 它 压 扁 成 3x3x64 的 一 维 
He, 然后 再 传 入 一 个 包含 64 个 神经 元 的 网 络 层 ， 由 于 我 们 要 识别 图 片 中 的 手写 数字 ， 其 对 应 
的 结果 有 10 种 ， 也 就 是 0~9， 因 此 最 后 我 们 还 添加 了 一 个 含有 10 个 神经 元 的 网 络 层 。 

把 图 片 数据 输入 网 络 ， 对 网 络 进行 训练 : 


| from keras.datasets import mnist 
from keras import layers 
from keras import models 


import numpy as np 


def get_data(): 


(train_images, train_labels), (test_images, test_labels) = mnist.load_data() 


train_images = train_images.reshape((train_images.shape[0], 28, 28, 1)).astype 
("fleat32") / 255 


test_images = test_images.reshape((test_images.shape[0], 28, 28, 1)).astype 
('float32') / 255 


def to label(labels): 
zero labels - np.zeros((labels.shape[0], 10), dtype-int) 
for index, label in enumerate(labels): 
zero_labels[index] [label] = 1 
return zero_labels 


train_labels = to_label(train_labels) 
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test_labels = to_label(test_labels) 

return (train_images, train_labels), (test_images, test_labels) 
(train_images, train_labels), (test_images, test_labels) = get_data() 
model.compile(optimizer-'rmsprop', loss-'categorical crossentropy', metrics= 

['accuracy']) 


model.fit(train, images, train labels, epochs - 5, batch, size-64) 


test loss, test acc - model.evaluate(test images, test labels) 


print("test loss: {}, test acc: ()".format(test loss, test acc)) 


上 面 的 代码 运行 后 ， 输 出 结果 如 图 8.38 Aras. 


图 8.38 结果 输出 


我 们 构造 的 卷 积 网 络 对 手写 数字 图 片 的 识别 准确 率 为 99%, 而 我 们 最 开始 使 用 的 网 络 对 图 
片 识 别 的 准确 率 是 979%。 也 就 是 说 , 最 简单 的 卷 积 网 络 , 对 图 片 的 识别 效果 也 要 比 普通 网 络 好 得 
多 。 能 取得 这 种 的 好 效果 ， 主 要 是 网 络 进行 了 两 种 特殊 操作 ， 分 别 是 Conv2D 和 MaxPooling2D。 

ERRE, 其 实 是 把 一 张大 图 片 分 解 成 好 多 个 小 部 分 , 然后 一 次 对 这 些小 部 分 进行 识别 , 我 
们 最 开始 实现 的 网 络 是 一 下 子 识 别 整 张大 图 片 ， 这 是 两 种 网 络 对 图 片 识别 精确 度 不 一 样 的 重要 
原因 。 通 常 ， 我 们 会 把 一 张 图 片 分 解 成 多 个 3x3 或 5x5 的 “小 片 ” 然后 分 别 识别 这 些小 片段 ， 
最 后 把 识别 的 结果 和 集合 在 一 起 输出 给 下 一 层 网 络 ， 如 图 8.39 所 示 。 

图 8.39 中 小 方 格 圈 中 的 区 域 就 是 我 们 抠 出 来 的 3x3 小 块 。 这 种 做 法 其 实 是 一 种 分 而 治之 的 
策略 ， 如 果 一 个 整体 很 难 攻 克 ， 那 么 我 就 把 整体 瓦解 成 多 个 弱小 的 局 部 ， 然 后 把 每 个 局 部 攻克 
了 ， 那 么 整体 就 攻克 了 。 这 种 做 法 在 图 像 识别 中 很 有 效 就 在 于 它 能 对 不 同 区 域 进行 识别 。 假 设 
识别 的 图 片 是 猫 脸 ， 那 么 我 们 就 可 以 把 猫 脸 分 解 成 耳 人 条 、 嘴 巴 、 眼睛、 胡子 等 多 个 部 位 去 各 上 自 
识别 ， 然 后 再 把 各 个 部 分 的 识别 结果 综合 起 来 作为 对 猫 脸 的 识别 。 
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Pa 
839 识别 的 结果 集合 


每 一 小 块 经 识别 后 ， 会 得 到 一 个 结果 向 量 ， 例 如 语句 : 


layers.Conv2D(32, (3,3), activation='relu', input_shape=(28,28,1)) 


它 表 示 把 一 个 28x28 像素 的 灰 度 图 片 (1 表示 颜色 深度 ， 对 于 灰 度 图 其 深度 用 一 个 数字 就 
可 以 表示 ， 对 于 RGB 图 ， 颜 色 深 度 需要 用 3 个 数字 [R,G,B] 表示 )， 分 解 成 多 个 3x3 的 小 块 , 每 
个 3x3 小 块 经 识别 后 输出 一 个 含有 32 个 元 素 的 一 维 向 量 。 这 个 一 维 向 量 被 称 为 过 滤器 ， 它 列 
含 着 对 图 片 的 识别 信息 ， 例 如 “这 部 分 对 应 猫 脸 的 嘴巴 ”。 

对 于 28x28 像素 的 图 片 ， 把 它 分 解 成 3x3 小 块 时 ， 这 些小 块 总 共有 26x26 个 。 我 们 先 看 
看 分 解 方法 。 假 定 我 们 有 一 个 5x5 的 大 图 片 ， 那 么 我 们 可 以 将 它 分 解 成 多 个 3x3 的 小 块 ， 如 
8.40 所 示 。 


LI: 
mmu 


图 8.40 图 片 分 解 
如 果 看 不 出 分 解 规律 ， 我 们 把 左边 大 图 片 的 每 个 小 格 标号 后 就 清楚 了 ， 左 边 图 片 编号 如 下 : 


1,2,3,4,5 
6,7,8,9,10 
11,12,13,14,15, 
16,17,18,19,20 
21,22,23,24,25 
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于 是 右边 第 一 行 第 一 小 块 对 应 的 编号 为 : 
1,2,3 

6,7,8 

11,12,13 


第 一 行 第 二 小 块 为 : 
2,3,4 

7,8,9 

12,13,14 


以 此 类 推 ， 一 个 28x28 像素 的 图 片 可 以 分 解 成 26 个 3x3 的 小 块 ， 每 个 小 块 又 计算 出 一 个 
含有 32 个 元 素 的 向 量 ， 这 个 计算 其 实 是 将 3x3 的 矩阵 乘 以 一 个 链 路 参数 矩阵 ， 它 与 我 们 前 面 
讲 过 的 数据 层 上 一 层 网 络 经 过 神经 元 链 路 输入 下 一 层 网 络 的 原理 是 一 样 的 ， 于 是 第 一 层 网 络 输 
出 的 结果 是 26x26x32 的 三 维 矩 阵 。 我 们 可 以 用 图 8.41 形 象 地 表示 网 络 输 出 结果 。 


高 


Ap——— 


输入 特征 图 
ee 3x3 输入 通道 
di 点 乘 卷 积 核 


8.41 网络 输出 结果 
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上 面 描述 的 操作 流程 就 叫 卷 积 。 接 下 来 我 们 看 看 另 一 种 操作 : max pooling。 从 我 们 的 代码 
中 看 到 ， 第 一 层 网 络 叫 Conv2D, 第 二 层 就 是 MaxPooling2D， 最 大 池 化 的 目标 是 把 由 卷 积 操作 
得 到 的 结果 进一步 “ 挤 压 ” 出 更 有 用 的 信息 ， 有 点 类 似 于 用 力 持 毛巾， 把 不 必要 的 水 分 给 挤 掩 。 
最 大 池 化 其 实 是 把 一 个 二 维 矩阵 进行 2x2 的 分 块 ， 这 部 分 跟前 面 描述 的 卷 积 很 像 ， 具 体操 作 如 
图 8.42 所 示 。 


Single depth slice 


做 最 大 池 化 


图 8.42 矩阵 分 块 


一 定 要 注意 上 面 的 分 块 与 卷 积 分 块 的 区 别 ， 上 面 分 出 的 块 与 块 之 间 是 没有 重 县 的， 而 卷 积 
分 出 的 3x3 小 块 之 间 是 相互 重合 的 ! 2x2 分 块 后 ， 把 每 块 中 的 最 大 值 抽出 来 ， 最 后 组 合成 右边 
的 小 块 。 注 意 ， 完 成 后 矩阵 的 维度 缩减 了 一 半 ， 即 原来 4x4 的 矩阵 变 成 了 2x2 的 矩阵 。 

回 到 我 们 的 代码 例子 ， 第 一 层 卷 积 网 络 输出 了 26x26x32 的 结果 ， 我 们 可 以 将 其 看 成 由 32 
个 26x26 个 二 维 矩 阵 的 集合 。 每 个 26x26 的 二 维和 矩阵 都 经 过 上 面 的 最 大 池 化 处 理 变 成 13x 13 
的 三 维和 矩阵 ， 因 此 经 过 第 二 层 最 大 池 化 后 ， 输 出 的 结果 是 13x13x32 的 矩阵 集合 ， 也 就 是 下 面 
代码 产生 了 32 个 13x13 的 矩阵 集合 : 


layers .MaxPooling2D (2,2) 


其 他 代码 以 此 类 推 。 卷 积 操作 产生 了 太 多 的 数据 ， 如 果 没 有 最 大 池 化 对 这 些 数据 进行 压缩， 
那么 网 络 的 运算 量 将 会 非常 巨大 ， 而 且 数 据 参数 过 于 元 余 就 非常 容易 导致 过 度 拟 合 。 

以 上 就 完成 了 我 们 的 数字 识别 模型 的 创建 ， 接 下 来 我 们 需要 实际 中 使 用 这 个 模型 进行 数字 
的 识别 。 


8.6.2 ”使 用 模型 进行 数字 识别 


之 前 我 们 已 经 介绍 了 如 何 利用 Keras 进行 卷 积 神经 网 络 的 创建 和 训练 。 接 下 来 ， 我 们 利用 
之 前 训练 得 到 的 模型 对 摄像 头 传 过 来 的 实时 数据 进行 数字 识别 判断 。 

首先 ， 我 们 需要 在 摄像 头 中 心 画 一 个 用 来 进行 数字 识别 的 矩形 框 ， 代 码 如 下 : 
actual_height, actual_width, _ = img.shape 
identify_height, identify_width = (300, 300) 
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pti = ((actual_width - identify_width) / 2, 
(actual_height - identify_height) / 2) 

pt2 = ((actual_width + identify_width) / 2, 
(actual_height + identify_height) / 2) 

cv2.rectangle(img, pti, pt2, (0, 255, 0), 2) 


将 图 像 转 换 为 二 值 图 ， 且 去 查找 当前 矩形 框 中 所 有 的 轮廓 : 


gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ] 
_, thresh = cv2.threshold(gray, 
80, 
255, 
cv2.THRESH BINARY INV * cv2.THRESH OTSU) 
contours, _ = cv2.findContours(thresh, 
cv2.RETR, TREE, 
cv2.CHAIN. APPROX. SIMPLE) [-2:] 


J 


取得 最 大 的 轮廓 并 为 轮廓 上 、 下 、 左 、 右 4 个 方向 添加 上 50 像素 的 黑色 框 以 便 后 面 更 好 
识别 : 


contour = max(contours, key=cv2.contourArea) 

X, y, W, h = cv2.boundingRect (contour) 

number img - thresh[y:y * h, x:x * w] 

number img = cv2.copyMakeBorder (number, img, 
50, 50, 50, 50, 
cv2.BORDER, CONSTANT , 
None, 0) 


将 图 片 压缩 成 28x28 像素 的 大 小 ， 并 设置 为 我 们 的 神经 网 络 需要 的 输入 类 型 : 


number_img = cv2.resize(number_img, (28, 28)) 


number_img = number_img.flatten() 
number img = number img.reshape(1, 28, 28, 1).astype('float32') / 255 


利用 Keras 对 获取 到 的 图 像 数 据 进 行 识别 判断 ， 然 后 执行 相应 操作 : 


ans = model.predict (number_img) 


ans = ans.tolist() 


num = ans[0].index(max(ans[0])) 
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x += pti[0] 
y += pti[1] 
cv2.rectangle(img, (x, y), (x * w, y + b), (0, 0, 255), 2) 
if ans[0] [num] > 0.8: 


global num2 count, num3 count 
if num == 
num2 count += 1 
if num2 count » 10: 
num2 count = 0 
print(" 识 别 到 2 T, #44") 
right_hand_up() 
elif num == 
num3_count += 1 
if num3_count > 10: 
num3_count = 0 
Print(" 识 别 到 3 了 ， 举 左手 ") 
left_hand_up() 


else: 


num2_count 


num3_count 


在 以 上 操作 中 ,我 们 对 识别 进行 了 防止 误 识别 的 处 理 ， 只 有 连续 识别 到 10 次 我 们 才 认 为 成 


功 识别 到 了 该 数字 。 
在 实际 使 用 中 ， 我 们 给 出 一 张 数字 2 的 图 片 ， 且 成 功 识 别 ， 机 器 人 举 起 了 右手 ， 如 图 8.43 
所 示 。 


E1843 数字 识别 结果 


[1] 
[2] 


[3] 
[4] 
[5] 
[6] 
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